Motion planner (MoveIt) and Task planner (Plansys) integration

2023-04-30 11:46:52 +00:00 · 2023-04-30 11:46:52 +00:00 · 000ddb4831
commit 000ddb4831
parent 0a735b87c9
43 changed files with 1402 additions and 379 deletions
--- a/env_components/assemble_component/CMakeLists.txt
+++ b/env_components/assemble_component/CMakeLists.txt
@ -1,58 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(assemble_component)
-
-if(NOT CMAKE_C_STANDARD)
-    set(CMAKE_C_STANDARD 99)
-endif()
-
-if(NOT CMAKE_CXX_STANDARD)
-    set(CMAKE_CXX_STANDARD 17)
-endif()
-
-set(CMAKE_CXX_STANDARD_REQUIRED True)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-find_package(ament_cmake REQUIRED)
-find_package(ament_cmake_ros REQUIRED)
-find_package(rbs_skill_interfaces REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(rclcpp_components REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(tf2_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(env_manager REQUIRED)
-
-add_library(assemble_state_component SHARED
-    src/assemble_state_component.cpp)
-target_compile_definitions(assemble_state_component
-    PRIVATE "COMPOSITION_BUILDING_DLL")
-ament_target_dependencies(assemble_state_component
-    "rbs_skill_interfaces"
-    "rclcpp"
-    "rclcpp_components"
-    "tf2_ros"
-    "tf2_msgs"
-    "geometry_msgs"
-    "env_manager")
-rclcpp_components_register_nodes(assemble_state_component "assemble_component::AssembleStateComponent")
-set(node_plugins "${node_plugins}assemble_component::AssembleStateComponent;$<TARGET_FILE:assemble_component>\n")
-
-install(TARGETS assemble_state_component
-    DESTINATION lib)
-
-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()
--- a/env_components/assemble_component/src/assemble_state_component.cpp
+++ b/env_components/assemble_component/src/assemble_state_component.cpp
@ -1,233 +0,0 @@
-#include <component_manager/service_component.hpp>
-
-#include <iostream>
-#include <memory>
-#include <string>
-#include <vector>
-
-#include <rclcpp/rclcpp.hpp>
-#include <tf2_ros/transform_listener.h>
-#include <tf2_ros/static_transform_broadcaster.h>
-#include <tf2_ros/buffer.h>
-#include <tf2_msgs/msg/tf_message.h>
-#include <geometry_msgs/msg/transform_stamped.hpp>
-#include <geometry_msgs/msg/pose_stamped.hpp>
-#include <geometry_msgs/msg/twist.hpp>
-
-#include <ament_index_cpp/get_package_share_directory.hpp>
-
-#include <rbs_skill_interfaces/srv/assemble_state.hpp>
-
-namespace assemble_component
-{
-
-const std::string ASSEMBLE_PACKAGE_NAME_PARAM = "assemble_package_name";
-const std::string ASSEMBLE_PREFIX_NAME = "ASSEMBLE";
-const double ASSEMBLE_POSITION_OFFSET = 0.5;
-const double ASSEMBLE_ORIENTATION_OFFSET = 0.5;
-
-/* TODO: implement sdf parse for assemble_solver.
- * */
-struct assemble_solver
-{
-    std::string package_name;
-    std::string assemble_name;
-    std::string part_name;
-    std::string workspace;
-    std::map<std::string, geometry_msgs::msg::PoseStamped> assemble_parts;
-};
-
-
-class AssembleStateComponent: public env_manager::component_manager
-    ::ServiceComponent<rbs_skill_interfaces::srv::AssembleState>
-{
-public:
-    AssembleStateComponent(const rclcpp::NodeOptions& options)
-        : env_manager::component_manager
-            ::ServiceComponent<rbs_skill_interfaces::srv::AssembleState>(options)
-    {
-        this->declare_parameter(ASSEMBLE_PACKAGE_NAME_PARAM, NULL); 
-        param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
-        auto pevcallback = [this](const rclcpp::Parameter & p) {
-            if (ASSEMBLE_PACKAGE_NAME_PARAM == p.get_name() && 
-                assemble_package_name_ != p.as_string() &&
-                current_state_ == AssembleReqState::NONE)
-                assemble_package_name_ = p.as_string();
-            else
-                RCLCPP_WARN(get_logger(), 
-                    "Unable to change param while assemble process in progress.");
-        };
-        cb_handle_ = param_subscriber_->add_parameter_callback(
-                    ASSEMBLE_PACKAGE_NAME_PARAM, pevcallback); 
-        tf_buffer_ = 
-            std::make_unique<tf2_ros::Buffer>(get_clock());
-        tf_listener_ = 
-            std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
-    }
-     
-    void callback(
-        std::shared_ptr<rbs_skill_interfaces::srv::AssembleState::Request> request,
-        std::shared_ptr<rbs_skill_interfaces::srv::AssembleState::Response> response)
-    {
-        auto state = static_cast<AssembleReqState>(request->req_kind);
-        auto call_status = false;
-        switch(state)
-        {
-            case AssembleReqState::NONE:
-                response->call_status = false;
-                return;
-            case AssembleReqState::INITIALIZE:
-                call_status = (current_state_ != AssembleReqState::NONE)? false: 
-                    on_initialize(request->assemble_name, request->part_name, request->workspace);
-                if (!call_status)
-                    RCLCPP_WARN_STREAM(get_logger(), "callback: " << "initialize error.");
-                break;
-            case AssembleReqState::VALIDATE:
-                call_status = (current_state_ == AssembleReqState::INITIALIZE);
-                response->validate_status = (call_status)? on_validate(): call_status;
-                if (!call_status)
-                    RCLCPP_WARN_STREAM(get_logger(), "callback: " << "validate error");
-                break;
-            case AssembleReqState::COMPLETE:
-                call_status = (current_state_ != AssembleReqState::VALIDATE)? false: on_complete();
-                if (!call_status)
-                    RCLCPP_WARN_STREAM(get_logger(), "callback: " << "complete process error.");
-                break;        
-            case AssembleReqState::CANCEL:
-                call_status = (current_state_ == AssembleReqState::NONE)? false: on_cancel();
-                if (!call_status)
-                    RCLCPP_WARN_STREAM(get_logger(), "callback: " << "complete process error.");
-                break;
-        }
-        response->call_status = call_status;
-    }
-
-    bool on_initialize(
-        const std::string &assemble_name, 
-        const std::string &part_name,
-        const std::string &workspace)
-    {
-        if (current_state_ != AssembleReqState::NONE)
-        {
-            RCLCPP_WARN_STREAM(get_logger(), "on_initialize: " << "assemble in progress");
-            return false;
-        }
-        try {
-            mt.lock();
-            assemble_ = (assemble_solver){ 
-                .package_name=assemble_package_name_, 
-                .assemble_name=assemble_name, 
-                .part_name=part_name, 
-                .workspace=workspace}; 
-            mt.unlock();
-        } catch (std::exception &ex) {
-            RCLCPP_ERROR_STREAM(get_logger(), "on_initialize: " << ex.what());
-            return false;
-        }
-        current_state_ = AssembleReqState::INITIALIZE;
-        tf_broadcaster_ = 
-            std::make_unique<tf2_ros::StaticTransformBroadcaster>(this);
-        send_transform(assemble_);
-
-        return true;
-    }
-    bool on_validate()
-    {
-        std::string frame_from = ASSEMBLE_PREFIX_NAME + assemble_.part_name; 
-        std::string frame_to = assemble_.part_name;
-
-        geometry_msgs::msg::TransformStamped t;
-        try {
-            t = tf_buffer_->lookupTransform(
-                frame_to, frame_from, tf2::TimePointZero);
-        } catch (const tf2::TransformException &ex) {
-            RCLCPP_WARN_STREAM(get_logger(),
-                "Could not transform " 
-                << frame_to << " to " << frame_from
-                << " " << ex.what());
-            return false;
-        }
-
-        auto pos_validate = 
-            t.transform.translation.x < ASSEMBLE_POSITION_OFFSET &&
-            t.transform.translation.y < ASSEMBLE_POSITION_OFFSET &&
-            t.transform.translation.z < ASSEMBLE_POSITION_OFFSET;
-        auto orient_validate =
-            t.transform.rotation.x < ASSEMBLE_ORIENTATION_OFFSET &&
-            t.transform.rotation.y < ASSEMBLE_ORIENTATION_OFFSET &&
-            t.transform.rotation.z < ASSEMBLE_ORIENTATION_OFFSET &&
-            t.transform.rotation.w < ASSEMBLE_ORIENTATION_OFFSET;
-        
-        return pos_validate && orient_validate;
-    }
-    bool on_complete()
-    {
-        try {
-            tf_broadcaster_.reset();
-        } catch (const std::exception &ex) {
-            RCLCPP_ERROR_STREAM(get_logger(), "on_complete: " << ex.what());
-            return false;
-        }
-        current_state_ = AssembleReqState::NONE;
-        assemble_ = {};
-        return true;
-    }
-    bool on_cancel()
-    {
-        return on_complete();
-    }
-
-protected:
-    enum AssembleReqState
-    {
-        NONE = -1,
-        INITIALIZE = 0,
-        VALIDATE = 1,
-        COMPLETE = 2,
-        CANCEL = 3
-    };
-
-private:
-
-    void send_transform(const assemble_solver &solver)
-    {
-        if (solver.assemble_parts.empty())
-        {
-            RCLCPP_WARN_STREAM(
-                get_logger(), "send_transform: " << "solver parts are empty");
-            return;
-        }
-        for (const auto& pt: solver.assemble_parts)
-        {
-            geometry_msgs::msg::TransformStamped t;
-            t.header.frame_id = solver.workspace;
-            t.child_frame_id = pt.first.c_str();
-            auto pose = pt.second.pose;
-            t.transform.translation.x = pose.position.x;
-            t.transform.translation.y = pose.position.y;
-            t.transform.translation.z = pose.position.z;
-            t.transform.rotation.x = pose.orientation.x;
-            t.transform.rotation.y = pose.orientation.y;
-            t.transform.rotation.z = pose.orientation.z;
-            t.transform.rotation.w = pose.orientation.w;
-            tf_broadcaster_->sendTransform(t);
-        }
-    }
-
-    std::string assemble_package_name_;
-    AssembleReqState current_state_;   
-
-    std::shared_ptr<rclcpp::ParameterEventHandler> param_subscriber_;
-    std::shared_ptr<rclcpp::ParameterCallbackHandle> cb_handle_;
-
-    std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
-    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
-    std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tf_broadcaster_;
-    assemble_solver assemble_;
-
-    std::mutex mt;
-};
-} // namespace assemble_component
-#include "rclcpp_components/register_node_macro.hpp"
-
-RCLCPP_COMPONENTS_REGISTER_NODE(assemble_component::AssembleStateComponent)
--- a/env_components/component_interfaces/CMakeLists.txt
+++ b/env_components/component_interfaces/CMakeLists.txt
@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.8)
+project(component_interfaces)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/DetectObject.srv"
+  DEPENDENCIES std_msgs)
+
+ament_package()
--- a/env_components/component_interfaces/package.xml
+++ b/env_components/component_interfaces/package.xml
@ -0,0 +1,20 @@
+<?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>component_interfaces</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <depend>std_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
--- a/env_components/component_interfaces/srv/DetectObject.srv
+++ b/env_components/component_interfaces/srv/DetectObject.srv
@ -0,0 +1,10 @@
+uint8 DETECT=0
+uint8 FOLLOW=1
+uint8 CANCEL=2
+
+string object_name
+uint8 req_kind
+---
+
+bool call_status
+string error_msg
--- a/env_components/scene_component/CMakeLists.txt
+++ b/env_components/scene_component/CMakeLists.txt
@ -0,0 +1,118 @@
+cmake_minimum_required(VERSION 3.5)
+project(scene_component)
+
+if(NOT CMAKE_C_STANDARD)
+    set(CMAKE_C_STANDARD 99)
+endif()
+
+if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 17)
+endif()
+
+set(CMAKE_CXX_STANDARD_REQUIRED True)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(ros_gz_bridge REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(env_manager REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(moveit_msgs REQUIRED)
+find_package(moveit_ros_planning REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(component_interfaces REQUIRED)
+
+find_package(ignition-transport11 REQUIRED)
+set(GZ_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR})
+
+find_package(ignition-msgs8 REQUIRED)
+set(GZ_MSGS_VER ${ignition-msgs8_VERSION_MAJOR})
+
+set(GZ_TARGET_PREFIX ignition)
+
+message(STATUS "Compiling against Ignition Fortress")
+
+
+add_library(ign_tf2_broadcaster_component SHARED
+    src/ign_tf2_broadcaster_component.cpp)
+target_compile_definitions(ign_tf2_broadcaster_component
+    PRIVATE "COMPOSITION_BUILDING_DLL")
+target_link_libraries(ign_tf2_broadcaster_component
+    ${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core
+    ${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core
+    )
+ament_target_dependencies(ign_tf2_broadcaster_component
+    "ros_gz_bridge"
+    "rclcpp"
+    "rclcpp_components"
+    "env_manager"
+    "geometry_msgs"
+    "tf2_ros")
+rclcpp_components_register_nodes(ign_tf2_broadcaster_component "scene_component::IgnTf2Broadcaster")
+set(node_plugins "${node_plugins}scene_component::IgnTf2Broadcaster;$<TARGET_FILE:scene_component>\n")
+
+add_library(ign_detect_object_component SHARED
+    src/ign_detect_object_component.cpp)
+target_compile_definitions(ign_detect_object_component
+  PRIVATE "COMPOSITION_BUILDING_DLL")
+target_link_libraries(ign_detect_object_component
+    ${GZ_TARGET_PREFIX}-msgs${GZ_MSGS_VER}::core
+    ${GZ_TARGET_PREFIX}-transport${GZ_TRANSPORT_VER}::core
+    )
+ament_target_dependencies(ign_detect_object_component
+    "ros_gz_bridge"
+    "rclcpp"
+    "rclcpp_components"
+    "env_manager"
+    "geometry_msgs"
+    "tf2_ros"
+    "component_interfaces")
+  rclcpp_components_register_nodes(ign_detect_object_component "scene_component::IgnDetectOpjectService")
+  set(node_plugins "${node_plugins}scene_component::IgnDetectObjectService;$<TARGET_FILE:scene_component>\n")
+
+add_library(moveit_scene_component SHARED
+    src/moveit_scene_component.cpp)
+target_compile_definitions(moveit_scene_component
+    PRIVATE "COMPOSITION_BUILDING_DLL")
+ament_target_dependencies(moveit_scene_component
+    "rclcpp"
+    "rclcpp_components"
+    "env_manager"
+    "geometry_msgs"
+    "tf2_ros"
+    "moveit_core"
+    "moveit_msgs"
+    "moveit_ros_planning"
+    "moveit_ros_planning_interface")
+rclcpp_components_register_nodes(moveit_scene_component "scene_component::MoveitSceneComponent")
+set(node_plugins "${node_plugins}scene_component::MoveitSceneComponent;$<TARGET_FILE:scene_component>\n")
+
+install(
+    TARGETS 
+        ign_detect_object_component
+        ign_tf2_broadcaster_component
+        moveit_scene_component
+    DESTINATION 
+        lib)
+
+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()
--- a/env_components/assemble_component/package.xml
+++ b/env_components/assemble_component/package.xml
@ -1,7 +1,7 @@
 <?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>assemble_component</name>
+  <name>scene_component</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
@ -10,13 +10,17 @@
  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>pkg-config</buildtool_depend>

-  <depend>rbs_skill_interfaces</depend>
+  <depend>image_transport</depend>
+  <depend>ros_gz_bridge</depend>
  <depend>rclcpp</depend>
  <depend>rclcpp_components</depend>
  <depend>geometry_msgs</depend>
-  <depend>tf2_ros</depend>
  <depend>env_manager</depend>
-  <depend>tf2_msgs</depend>
+
+  <depend condition="$GZ_VERSION == fortress or $IGNITION_VERSION == fortress">ignition-msgs8</depend>
+  <depend condition="$GZ_VERSION == fortress or $IGNITION_VERSION == fortress">ignition-transport11</depend>
+  <depend condition="$GZ_VERSION == '' and $IGNITION_VERSION == ''">ignition-msgs8</depend>
+  <depend condition="$GZ_VERSION == '' and $IGNITION_VERSION == ''">ignition-transport11</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
--- a/env_components/scene_component/src/ign_detect_object_component.cpp
+++ b/env_components/scene_component/src/ign_detect_object_component.cpp
@ -0,0 +1,109 @@
+#include "component_manager/service_component.hpp"
+#include "component_interfaces/srv/detect_object.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_msgs/msg/tf_message.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+
+#include <ignition/transport/Node.hh>
+#include <ros_gz_bridge/convert.hpp>
+
+#define IGN_DEFAULT_BASE_FRAME "world"
+#define IGN_DEFAULT_WORLD_NAME "mir"
+
+namespace scene_component
+{
+
+class IgnDetectObjectService: public 
+  env_manager::component_manager::ServiceComponent<component_interfaces::srv::DetectObject>
+{
+  public:
+    IgnDetectObjectService(const rclcpp::NodeOptions &options)
+      : env_manager::component_manager::ServiceComponent<component_interfaces::srv::DetectObject>(options)
+    {
+      std::string ign_topic_name = "/world/" + std::string(IGN_DEFAULT_WORLD_NAME) + "/dynamic_pose/info";
+      _tf2_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(this);
+      _ign_node = std::make_shared<ignition::transport::Node>();
+      _ign_node->Subscribe(ign_topic_name, &IgnDetectObjectService::on_pose, this);
+    }
+
+    void callback(
+      std::shared_ptr<component_interfaces::srv::DetectObject::Request> request,
+      std::shared_ptr<component_interfaces::srv::DetectObject::Response> response)
+    {
+      auto kind = static_cast<DetectObjectReqKind>(request->req_kind);
+      bool status = false;
+      switch(kind)
+      {
+        case DETECT: {
+          (void) kind;
+          break;
+        } case FOLLOW: {
+          auto ok = std::find(_follow_frames.begin(), _follow_frames.end(), 
+              request->object_name) == _follow_frames.end();
+          if (ok)
+            _follow_frames.push_back(request->object_name);
+          status = ok; 
+          break;
+        } case CANCEL: {
+          auto it = std::find(_follow_frames.begin(), _follow_frames.end(), 
+              request->object_name);
+          auto ok = it != _follow_frames.end();
+          if (ok)
+            _follow_frames.erase(it);
+          status = ok;
+          break;
+        }
+      }
+      response->call_status = status;
+    }
+
+  private:
+    enum DetectObjectReqKind
+    {
+      DETECT=0,
+      FOLLOW=1,
+      CANCEL=2
+    };
+
+    void on_pose(const ignition::msgs::Pose_V &pose_v)
+    {
+      auto poses = pose_v.pose();
+      for (const auto& obj: _follow_frames)
+      {
+        auto it = std::find_if(poses.begin(), poses.end(), 
+          [&obj](const auto& pose){ return pose.name() == obj;});
+        if (it == poses.end())
+          continue;
+
+        geometry_msgs::msg::PoseStamped rpose;
+        ros_gz_bridge::convert_gz_to_ros(*it, rpose);
+        geometry_msgs::msg::TransformStamped t;
+
+        t.header.stamp = this->get_clock()->now();
+        t.header.frame_id = IGN_DEFAULT_BASE_FRAME;
+        t.child_frame_id = it->name();
+        t.transform.translation.x = rpose.pose.position.x;
+        t.transform.translation.y = rpose.pose.position.y;
+        t.transform.translation.z = rpose.pose.position.z;
+        t.transform.rotation.x = rpose.pose.orientation.x;
+        t.transform.rotation.y = rpose.pose.orientation.y;
+        t.transform.rotation.z = rpose.pose.orientation.z;
+        t.transform.rotation.w = rpose.pose.orientation.w;
+
+        _tf2_broadcaster->sendTransform(t);
+      }
+    }
+
+    std::vector<std::string> _follow_frames;
+    std::unique_ptr<tf2_ros::TransformBroadcaster> _tf2_broadcaster;
+    std::shared_ptr<ignition::transport::Node> _ign_node;
+
+};
+
+} // namespace scene_component
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::IgnDetectObjectService)
--- a/env_components/scene_component/src/ign_tf2_broadcaster_component.cpp
+++ b/env_components/scene_component/src/ign_tf2_broadcaster_component.cpp
@ -0,0 +1,72 @@
+#include "component_manager/node_component.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_msgs/msg/tf_message.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+
+#include <ignition/transport/Node.hh>
+#include <ros_gz_bridge/convert.hpp>
+
+namespace scene_component
+{
+
+const std::string DEFAULT_NODE_NAME = "ign_tf2_broadcaster";
+const std::string DEFAULT_IGN_WORLD_NAME = "mir";
+const std::string DEFAULT_BASE_FRAME_NAME = "world";
+
+class IgnTf2Broadcaster: public env_manager::component_manager::NodeComponent
+{
+public:
+    IgnTf2Broadcaster(const rclcpp::NodeOptions & options)
+        : env_manager::component_manager::NodeComponent(options)
+    {
+        _follow_frames = {"box1", "box2", "box3", "box4", "box5", "box6"};
+        std::string topic_name = "/world/" + DEFAULT_IGN_WORLD_NAME + "/dynamic_pose/info";
+        // init broadcaster
+        _tf2_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+        _ign_node = std::make_shared<ignition::transport::Node>();
+        _ign_node->Subscribe(topic_name, &IgnTf2Broadcaster::on_pose, this);
+    }
+
+private:
+    void on_pose(const ignition::msgs::Pose_V& pose_v)
+    {
+        for (const auto &it: pose_v.pose())
+        {   
+            if (std::find(_follow_frames.begin(), _follow_frames.end(), it.name()) == _follow_frames.end())
+                continue;
+             
+            geometry_msgs::msg::PoseStamped rpose;
+            ros_gz_bridge::convert_gz_to_ros(it, rpose);
+            
+            geometry_msgs::msg::TransformStamped t;
+
+            t.header.stamp = this->get_clock()->now();
+            t.header.frame_id = DEFAULT_BASE_FRAME_NAME;
+            t.child_frame_id = it.name();
+
+            t.transform.translation.x = rpose.pose.position.x;
+            t.transform.translation.y = rpose.pose.position.y;
+            t.transform.translation.z = rpose.pose.position.z;
+
+            t.transform.rotation.x = rpose.pose.orientation.x;
+            t.transform.rotation.y = rpose.pose.orientation.y;
+            t.transform.rotation.z = rpose.pose.orientation.z;
+            t.transform.rotation.w = rpose.pose.orientation.w;
+
+            RCLCPP_ERROR(get_logger(), "Provide transform for frame: %s", it.name().c_str());
+           
+            _tf2_broadcaster->sendTransform(t);
+        }
+    }
+
+    std::unique_ptr<tf2_ros::TransformBroadcaster> _tf2_broadcaster;
+    std::shared_ptr<ignition::transport::Node> _ign_node;
+    std::vector<std::string> _follow_frames;
+
+};
+} // namespace scene_component
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::IgnTf2Broadcaster)
--- a/env_components/scene_component/src/moveit_scene_component.cpp
+++ b/env_components/scene_component/src/moveit_scene_component.cpp
@ -0,0 +1,115 @@
+#include <rclcpp/rclcpp.hpp>
+ 
+#include <component_manager/node_component.hpp>
+
+#include <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/moveit_cpp/planning_component.h>
+
+#include <geometry_msgs/msg/point_stamped.h>
+
+#include <urdf_parser/urdf_parser.h>
+#include <geometric_shapes/shape_operations.h>
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "tf2/exceptions.h"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.h"
+
+using namespace std::chrono_literals;
+
+namespace scene_component
+{
+
+const std::string DEFAULT_PLANNING_GOUP_NAME = "panda_arm"; 
+const std::string DEFAULT_SUBNODE_NS = "moveit_cpp_node";
+const std::string DEFAULT_BASE_FRAME = "world";
+
+class MoveitSceneComponent: public env_manager::component_manager::NodeComponent
+{
+public:
+    MoveitSceneComponent(const rclcpp::NodeOptions& options)
+        : env_manager::component_manager::NodeComponent(options)
+    {
+        rclcpp::NodeOptions node_options;
+        node_options.automatically_declare_parameters_from_overrides(true);
+        
+        node_ = rclcpp::Node::make_shared(DEFAULT_SUBNODE_NS, "", node_options);
+
+        moveit_cpp_ptr_ = std::make_shared<moveit_cpp::MoveItCpp>(node_);
+        moveit_cpp_ptr_->getPlanningSceneMonitor()->providePlanningSceneService();
+        planning_components_ = std::make_shared<moveit_cpp::PlanningComponent>(
+                    DEFAULT_PLANNING_GOUP_NAME, moveit_cpp_ptr_);
+        robot_model_ptr_ = moveit_cpp_ptr_->getRobotModel();
+
+        tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+        timer_ = this->create_wall_timer(1s, std::bind(&MoveitSceneComponent::on_timer, this));
+    }
+
+private:
+    void on_timer()
+    {
+        auto available_frames = tf_buffer_->getAllFrameNames();
+        auto robot_link_names = robot_model_ptr_->getLinkModelNames();
+
+        for (const auto &frame: available_frames)
+        {
+            if (std::find(robot_link_names.begin(), robot_link_names.end(), frame) != robot_link_names.end())
+                continue;
+
+            geometry_msgs::msg::TransformStamped t;
+            try {
+                t = tf_buffer_->lookupTransform(
+                    DEFAULT_BASE_FRAME, frame,
+                    tf2::TimePointZero);
+            } catch (const tf2::TransformException &ex)
+            {
+                RCLCPP_WARN(get_logger(), "Could not transform %s to %s: %s",
+                    frame.c_str(), DEFAULT_BASE_FRAME.c_str(), ex.what());
+            }
+
+            moveit_msgs::msg::CollisionObject co;
+            co.header.frame_id = robot_model_ptr_->getRootLink()->getName();
+            co.id = frame;
+
+            //TODO: replace with stl model
+            shape_msgs::msg::SolidPrimitive obj;
+            obj.type = obj.BOX;
+            obj.dimensions = {0.1, 0.1, 0.1};
+
+            geometry_msgs::msg::Pose obj_pose;
+            obj_pose.position.x = t.transform.translation.x;
+            obj_pose.position.y = t.transform.translation.y;
+            obj_pose.position.z = t.transform.translation.z;
+
+            obj_pose.orientation.x = t.transform.rotation.x;
+            obj_pose.orientation.y = t.transform.rotation.y;
+            obj_pose.orientation.z = t.transform.rotation.z;
+            obj_pose.orientation.w = t.transform.rotation.w;
+
+            co.primitives.push_back(obj);
+            co.primitive_poses.push_back(obj_pose);
+            co.operation = co.ADD;
+
+            {
+                planning_scene_monitor::LockedPlanningSceneRW scene(moveit_cpp_ptr_->getPlanningSceneMonitor());
+                scene->processCollisionObjectMsg(co);
+            }
+        }
+    }
+
+    rclcpp::Node::SharedPtr node_;
+    std::shared_ptr<moveit_cpp::MoveItCpp> moveit_cpp_ptr_;
+    moveit::core::RobotModelConstPtr robot_model_ptr_;
+    std::shared_ptr<moveit_cpp::PlanningComponent> planning_components_;
+    std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
+    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+    rclcpp::TimerBase::SharedPtr timer_{nullptr};
+
+
+};
+} // namesapce scene_component
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::MoveitSceneComponent)
--- a/env_manager/config/environments/ground_true.lua
+++ b/env_manager/config/environments/ground_true.lua
@ -2,18 +2,13 @@
 GROUND_TRUE = {
    namespace = "ground_true",
    components = {
-        --[[
-        talker_node = {
-            lib = "libpub_component.so",
-            class = "pub_component::Publisher",
-        },
-        service_node = {
-            lib = "libsrv_component.so",
-            class = "srv_component::Service"
-        },]]--
        camera_node = {
            lib = "libign_camera_component.so",
            class = "camera_component::IgnCameraComponent"
        },
+        scene_component = {
+            lib = "libign_detect_object_component.so",
+            class = "scene_component::IgnDetectObjectService"
+        },
    }
 }
--- a/env_manager/config/nodes.lua
+++ b/env_manager/config/nodes.lua
@ -1,18 +1,12 @@
 NODES = {
-    --[[
-    talker_node = {
-        name = "talker",
-        type = "Publisher",
-        msg_type = "std_msgs/String",
-    },
-    service_node = {
-        name = "add_two_ints",
-        type = "Service",
-        msg_type = "example_interfaces/AddTwoInts"
-    },]]--
    camera_node = {
        name = "camera_node",
        type = "Publisher",
        msg_type = "sensor_msgs/Image"
    },
+    scene_component = {
+        name = "scene_component",
+        type = "Service",
+        msg_type = "component_interfaces/DetectObject"
+    },
 }
--- a/env_manager/include/component_manager/node_component.hpp
+++ b/env_manager/include/component_manager/node_component.hpp
@ -0,0 +1,36 @@
+#ifndef ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__
+#define ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__
+
+#include "component_manager/visibility_control.h"
+
+#include "rclcpp/node.hpp"
+
+namespace env_manager
+{
+namespace component_manager
+{
+
+const std::string DEFAULT_NODE_NODE_NAME = "env_manager_node";
+
+//TODO: interhit all another components from this node_component
+class NodeComponent: public rclcpp::Node
+{
+public:
+    explicit NodeComponent(const rclcpp::NodeOptions& options)
+        :Node(DEFAULT_NODE_NODE_NAME, options)
+    {
+        auto ret = rcutils_logging_set_logger_level(
+            get_logger().get_name(), RCUTILS_LOG_SEVERITY_FATAL);
+        if (ret != RCUTILS_RET_OK)
+        {
+            RCLCPP_ERROR(get_logger(),
+                "Error setting severity: %s", rcutils_get_error_string().str);
+            rcutils_reset_error();
+        }
+    }
+};
+
+} // namesapce component_manager
+} // namesapce env_manager
+
+#endif // ENV_MANAGER__COMPONENT_MANAGER__NODE_COMPONENT_HPP__ 
--- a/env_manager/include/config/options.hpp
+++ b/env_manager/include/config/options.hpp
@ -33,7 +33,8 @@ struct NodeOption
        Publisher,
        Subscriber,
        Service,
-        Client
+        Client,
+        Node
    };

    static NodeOption create_option(
--- a/env_manager/package.xml
+++ b/env_manager/package.xml
@ -3,9 +3,9 @@
 <package format="3">
  <name>env_manager</name>
  <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>env_manager</description>
  <maintainer email="rom.andrianov1984@gmail.com">splinter1984</maintainer>
-  <license>TODO: License declaration</license>
+ <license>Apache-2.0</license> 

  <buildtool_depend>ament_cmake_ros</buildtool_depend>

--- a/env_manager/src/component_manager/component_manager.cpp
+++ b/env_manager/src/component_manager/component_manager.cpp
@ -1,4 +1,5 @@
 #include "component_manager/component_manager.hpp"
+#include "component_manager/node_component.hpp"
 #include "component_manager/publisher_component.hpp"
 #include "component_manager/subscriber_component.hpp"
 #include "component_manager/service_component.hpp"
@ -50,6 +51,9 @@ static rclcpp::NodeOptions create_options(
        case config::NodeOption::NodeType::Client:
            args.push_back(DEFAULT_CLIENT_NAME + ":=" + node_opts.name);
            break;
+        case config::NodeOption::NodeType::Node:
+            args.erase(args.end());
+            break;
    }

    opts.arguments(args);
@ -87,8 +91,11 @@ void ComponentManager::register_components(
        auto loader = new class_loader::ClassLoader(comp.library);
        auto classes = 
            loader->getAvailableClasses<rclcpp_components::NodeFactory>();
+
        for (auto clazz: classes)
        {
+            if (clazz != class_name)
+                continue;
            rclcpp::NodeOptions opts = create_default_options(ns);
            if (clazz == class_name)
            {
--- a/env_manager/src/config/options.cpp
+++ b/env_manager/src/config/options.cpp
@ -50,6 +50,7 @@ NodeOption::node_type_remap =
        {"Subscriber", NodeOption::NodeType::Subscriber},
        {"Service", NodeOption::NodeType::Service},
        {"Client", NodeOption::NodeType::Client},
+        {"Node", NodeOption::NodeType::Node},
    };

 EnvManagerOption EnvManagerOption::create_option(
--- a/rbs_bt_executor/CMakeLists.txt
+++ b/rbs_bt_executor/CMakeLists.txt
@ -5,7 +5,6 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
 endif()

-
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
@ -19,6 +18,7 @@ find_package(ament_index_cpp REQUIRED)
 find_package(rbs_skill_interfaces REQUIRED)
 find_package(behavior_tree REQUIRED)
 find_package(control_msgs REQUIRED)
+find_package(component_interfaces REQUIRED)

 if (NOT CMAKE_CXX_STANDARD)
    set(CMAKE_CXX_STANDARD 17)
@ -38,6 +38,7 @@ set(dependencies
    behavior_tree
    std_msgs
    control_msgs
+    component_interfaces
 )

 include_directories(include)
@ -57,13 +58,17 @@ list(APPEND plugin_libs rbs_skill_move_joint_state)
 add_library(rbs_add_planning_scene_object SHARED src/AddPlanningSceneObject.cpp)
 list(APPEND plugin_libs rbs_add_planning_scene_object)

+add_library(rbs_assemble_process_state SHARED src/AssembleProcessState.cpp)
+list(APPEND plugin_libs rbs_assemble_process_state)
+
+add_library(rbs_detect_object SHARED src/DetectObject.cpp)
+list(APPEND plugin_libs rbs_detect_object)
+
 foreach(bt_plugin ${plugin_libs})
    ament_target_dependencies(${bt_plugin} ${dependencies})
    target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
 endforeach()

-
-
 install(DIRECTORY launch bt_trees config DESTINATION share/${PROJECT_NAME})

 install(TARGETS
--- a/rbs_bt_executor/bt_trees/assemble.xml
+++ b/rbs_bt_executor/bt_trees/assemble.xml
@ -0,0 +1,57 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="Assemble">
+  <BehaviorTree ID="Assemble">
+    <Sequence>
+      <Action ID="DetectObject" ReqKind="FOLLOW" ObjectName="${arg0}"
+        server_name="/ground_true/scene_component" server_timeout="1000"/>
+
+      <Action ID="AssembleState" StateKind="INITIALIZE" PartName="${arg0}" 
+        AssembleName="${arg2}" WorkspaceName="${arg3}" server_name="/assemble_state" 
+        server_timeout="1000"/>
+      
+      <Action ID="GetPickPlacePoses" ObjectName="${arg0}" GraspDirection = "y" PlaceDirection = "z" 
+        GraspPose="{GraspPose}" PostGraspPose="{PostGraspPose}" PostPostGraspPose="{PostPostGraspPose}" 
+        PreGraspPose="{PreGraspPose}" PlacePose="{PlacePose}" PrePlacePose="{PrePlacePose}"
+        PostPlacePose="{PostPlacePose}" server_name="/get_pick_place_pose_service" server_timeout="1000"/>
+      
+      <Action ID="MoveToPose"  robot_name="${arg4}" pose="{PreGraspPose}" 
+        server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
+      
+      <Action ID="MoveToPose"  robot_name="${arg4}" pose="{GraspPose}" 
+        server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
+      
+      <Action ID="GripperControl" pose = "close" server_name="/gripper_control" 
+        server_timeout="10" cancel_timeout="5" />
+      
+      <Action ID="MoveToPose"  robot_name="${arg4}" pose="{PostGraspPose}" 
+        server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
+      
+      <Action ID="MoveToPose"  robot_name="${arg4}" pose="{PostPostGraspPose}" 
+        server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
+      
+      <Action ID="MoveToPose"  robot_name="${arg4}" pose="{PrePlacePose}" 
+        server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
+      
+      <Action ID="MoveToPose"  robot_name="${arg4}" pose="{PlacePose}" 
+        server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
+      
+      <Action ID="GripperControl" pose = "open" server_name="/gripper_control" 
+        server_timeout="10" cancel_timeout="5" />
+      
+      <Action ID="MoveToPose"  robot_name="${arg4}" pose="{PostPlacePose}" 
+        server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />    
+      `
+      <Action ID="AssembleState" StateKind="VALIDATE" PartName="${arg0}" 
+        AssembleName="${arg2}" WorkspaceName="${arg3}" server_name="/assemble_state" 
+        server_timeout="1000"/>
+
+      <Action ID="AssembleState" StateKind="COMPLETE" PartName="${arg0}" 
+        AssembleName="${arg2}" WorkspaceName="${arg3}" server_name="/assemble_state"
+        server_timeout="1000"/>
+
+      <Action ID="DetectObject" ReqKind="CANCEL" ObjectName="${arg0}"
+        server_name="/ground_true/scene_component" server_timeout="1000"/>
+
+    </Sequence>
+  </BehaviorTree>
+</root>
--- a/rbs_bt_executor/package.xml
+++ b/rbs_bt_executor/package.xml
@ -10,6 +10,7 @@
  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rbs_skill_interfaces</depend>
+  <depend>component_interfaces</depend>
  <depend>behavior_tree</depend>
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
--- a/rbs_bt_executor/src/AssembleProcessState.cpp
+++ b/rbs_bt_executor/src/AssembleProcessState.cpp
@ -0,0 +1,67 @@
+#include <string>
+#include <behavior_tree/BtService.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include "rbs_skill_interfaces/srv/assemble_state.hpp"
+
+using AssembleStateSrv = rbs_skill_interfaces::srv::AssembleState;
+
+class AssembleState : public BtService<AssembleStateSrv>
+{
+public:
+  AssembleState(const std::string & name, const BT::NodeConfiguration &config)
+  : BtService<AssembleStateSrv>(name, config)  
+  {
+    RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
+  }
+
+  AssembleStateSrv::Request::SharedPtr populate_request()
+  {
+    auto request = std::make_shared<AssembleStateSrv::Request>();
+    auto state_kind = getInput<std::string>("StateKind").value();
+    request->req_kind = -1;
+
+    if (state_kind == "INITIALIZE")
+      request->req_kind = 0;
+    else if (state_kind == "VALIDATE")
+      request->req_kind = 1;
+    else if (state_kind == "COMPLETE")
+      request->req_kind = 2;
+
+    auto assemble_name = getInput<std::string>("AssembleName").value();
+    auto part_name = getInput<std::string>("PartName").value();
+    auto workspace = getInput<std::string>("WorkspaceName").value();
+
+    request->assemble_name = assemble_name;
+    request->part_name = part_name;
+    request->workspace = workspace;
+
+    return request;
+  }
+
+  BT::NodeStatus handle_response(AssembleStateSrv::Response::SharedPtr response)
+  { 
+    // TODO: return bad status on validate process return false state
+    return (response->call_status = true)? BT::NodeStatus::SUCCESS: BT::NodeStatus::FAILURE;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<std::string>("StateKind"),
+      BT::InputPort<std::string>("AssembleName"),
+      BT::InputPort<std::string>("PartName"),
+      BT::InputPort<std::string>("WorkspaceName")
+    });
+  }
+
+private:
+  std::map<std::string, int> assemble_states_;
+};
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<AssembleState>("AssembleState");
+}
--- a/rbs_bt_executor/src/DetectObject.cpp
+++ b/rbs_bt_executor/src/DetectObject.cpp
@ -0,0 +1,51 @@
+#include <string>
+#include <behavior_tree/BtService.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <component_interfaces/srv/detect_object.hpp>
+
+using DetectObjectSrv = component_interfaces::srv::DetectObject;
+class DetectObject: public BtService<DetectObjectSrv>
+{
+public:
+  DetectObject(const std::string &name, const BT::NodeConfiguration &config)
+  : BtService<DetectObjectSrv>(name, config)
+  {
+    RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
+  }
+
+  DetectObjectSrv::Request::SharedPtr populate_request()
+  {
+    auto request = std::make_shared<DetectObjectSrv::Request>();
+    auto req_kind = getInput<std::string>("ReqKind").value();
+    if (req_kind == "DETECT")
+      request->req_kind = 0;
+    else if (req_kind == "FOLLOW")
+      request->req_kind = 1;
+    else
+      request->req_kind = 2;
+    request->object_name = getInput<std::string>("ObjectName").value();
+    return request;
+  }
+
+  BT::NodeStatus handle_response(DetectObjectSrv::Response::SharedPtr response)
+  {
+    return response->call_status? BT::NodeStatus::SUCCESS: BT::NodeStatus::FAILURE;
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<std::string>("ReqKind"),
+      BT::InputPort<std::string>("ObjectName"),
+    });
+  }
+};
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<DetectObject>("DetectObject");
+}
+
--- a/rbs_simulation/launch/rbs_simulation.launch.py
+++ b/rbs_simulation/launch/rbs_simulation.launch.py
@ -380,16 +380,16 @@ def generate_launch_description():
        ]
    )

-    move_topose_action_server = Node(
-        package="rbs_skill_servers",
-        executable="move_topose_action_server",
-        parameters=[
-            robot_description,
-            robot_description_semantic,
-            robot_description_kinematics,
-            {"use_sim_time": use_sim_time},
-        ]
-    )
+    #move_topose_action_server = Node(
+    #    package="rbs_skill_servers",
+    #    executable="move_topose_action_server",
+    #    parameters=[
+    #        robot_description,
+    #        robot_description_semantic,
+    #        robot_description_kinematics,
+    #        {"use_sim_time": use_sim_time},
+    #    ]
+    #)

    move_cartesian_path_action_server = Node(
        package="rbs_skill_servers",
@ -425,6 +425,16 @@ def generate_launch_description():
        ]
    )

+    assemble_state = Node(
+        package="rbs_skill_servers",
+        executable="assemble_state_service_server",
+        output="screen",
+        parameters=[
+            {'assemble_prefix': 'ASSEMBLE_'},
+            {'assemble_dir': '/home/splinter1984/tmp_ws/src/robossembler-ros2/rbs_task_planner/example/sdf_models/'}
+        ]
+    )
+    
    # add_planning_scene_object = Node(
    #     package="rbs_skill_servers",
    #     executable="add_planning_scene_object_service",
@ -452,6 +462,7 @@ def generate_launch_description():
        move_cartesian_path_action_server,
        move_joint_state_action_server,
        grasp_pose_loader,
+        assemble_state,
        #add_planning_scene_object
    ]

--- a/rbs_skill_interfaces/srv/AssembleState.srv
+++ b/rbs_skill_interfaces/srv/AssembleState.srv
@ -9,9 +9,6 @@ uint8 VALIDATE=1
 # COMPLETE - finalize assemble process
 uint8 COMPLETE=2

-# CANCEL - force finalize assemble process
-uint8 CANCEL=3
-
 string assemble_name
 string part_name
 uint8 req_kind
@ -23,6 +20,9 @@ string workspace

 bool call_status

+# wich assemble is asociate with response status
+string assemble_name
+
 # on error
 string error_msg

--- a/rbs_skill_servers/CMakeLists.txt
+++ b/rbs_skill_servers/CMakeLists.txt
@ -6,9 +6,9 @@ if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
 endif()

-# Default to C++14
+# Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
 endif()

 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@ -30,6 +30,16 @@ find_package(rbs_skill_interfaces REQUIRED)
 find_package(rmw REQUIRED)
 find_package(tf2_eigen REQUIRED)
 find_package(tf2_msgs REQUIRED)
+find_package(tinyxml2_vendor REQUIRED)
+find_package(TinyXML2 REQUIRED)
+
+include_directories(SYSTEM
+  ${TINYXML2_INCLUDE_DIR}
+)
+
+link_directories(
+  ${TINYXML2_LIBRARY_DIRS}
+)

 set(deps
  rclcpp
@ -44,6 +54,7 @@ set(deps
  rbs_skill_interfaces
  tf2_eigen
  tf2_msgs
+  tinyxml2_vendor
 )

 if(BUILD_TESTING)
@ -74,9 +85,6 @@ ament_target_dependencies(pick_place_pose_loader ${deps})
 rclcpp_components_register_node(gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer" EXECUTABLE gripper_control_action_server)
 rclcpp_components_register_node(pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer" EXECUTABLE pick_place_pose_loader_service_server)

-
-
-
 add_executable(move_to_joint_states_action_server src/move_to_joint_states_action_server.cpp)
 ament_target_dependencies(move_to_joint_states_action_server ${deps})

@ -92,6 +100,12 @@ ament_target_dependencies(move_cartesian_path_action_server ${deps})
 add_executable(add_planning_scene_object_service src/add_planning_scene_objects_service.cpp)
 ament_target_dependencies(add_planning_scene_object_service ${deps})

+add_library(assemble_state_server SHARED src/assemble_state_server.cpp)
+target_compile_definitions(assemble_state_server
+  PRIVATE "ASSEMBLE_STATE_SERVER_CPP_BUILDING_DLL")
+ament_target_dependencies(assemble_state_server ${deps})
+target_link_libraries(assemble_state_server ${TINYXML2_LIBRARY})
+rclcpp_components_register_node(assemble_state_server PLUGIN "AssembleStateServer" EXECUTABLE assemble_state_service_server)

 install(
  DIRECTORY include/
@ -106,6 +120,7 @@ install(
  move_to_joint_states_action_server
  move_cartesian_path_action_server
  add_planning_scene_object_service
+  assemble_state_server
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION lib/${PROJECT_NAME}
--- a/rbs_skill_servers/src/assemble_state_server.cpp
+++ b/rbs_skill_servers/src/assemble_state_server.cpp
@ -0,0 +1,245 @@
+#include <iostream>
+#include <memory>
+#include <string>
+#include <vector>
+#include <filesystem>
+
+#include <tinyxml2.h>
+#include <rclcpp/rclcpp.hpp>
+
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_ros/buffer.h>
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/twist.h>
+
+#include <rbs_skill_interfaces/srv/assemble_state.hpp>
+
+#define ASSEMBLE_PREFIX_PARAM_NAME "assemble_prefix"
+#define SERVICE_NAME "assemble_state"
+
+#define ASSEMBLE_DIR_PARAM_NAME "assemble_dir"
+#define ASSEMBLE_POSITION_OFFSET 0.5
+#define ASSEMBLE_ORIENTATION_OFFSET 0.5
+
+#define ASSEMBLE_SDF_PATH(package_dir, assemble_name) \
+  (package_dir) + "/models/" + (assemble_name) + "/model.sdf"
+
+static inline geometry_msgs::msg::Pose get_pose(
+  const std::vector<std::string> & tokens)
+{
+  geometry_msgs::msg::Pose p;
+  p.position.set__x(std::stod(tokens.at(0)));
+  p.position.set__y(std::stod(tokens.at(1)));
+  p.position.set__z(std::stod(tokens.at(2)));
+  p.orientation.set__x(std::stod(tokens.at(3)));
+  p.orientation.set__y(std::stod(tokens.at(4)));
+  p.orientation.set__z(std::stod(tokens.at(5)));
+
+  return p;
+}
+
+static inline geometry_msgs::msg::PoseStamped get_pose_stamped(
+  const std::string & pose_string)
+{
+  std::stringstream ss(pose_string);
+  std::istream_iterator<std::string> begin(ss);
+  std::istream_iterator<std::string> end;
+  std::vector<std::string> tokens(begin, end);
+  
+  geometry_msgs::msg::PoseStamped ps;
+  ps.set__pose(get_pose(tokens));
+  return ps;
+}
+
+static std::vector<geometry_msgs::msg::PoseStamped> get_assemble_poses(
+  const std::string & assemble_name, const std::string & part_name, 
+  const std::string& package_dir, const std::string & assemble_prefix)
+{
+  std::vector<geometry_msgs::msg::PoseStamped> result;
+  std::filesystem::path sdf_path = package_dir + assemble_name + ".sdf";
+
+  tinyxml2::XMLDocument doc;
+  doc.LoadFile(sdf_path.c_str());
+  auto model = doc.RootElement()->FirstChildElement("model");
+  auto joint = model->FirstChildElement("joint");
+
+  while (joint)
+  {
+    auto frame_id = joint->FirstChildElement("child")->GetText();
+    if (frame_id != part_name)
+      continue;
+    auto ps = get_pose_stamped(joint->FirstChildElement("pose")->GetText()); 
+    ps.header.set__frame_id(assemble_prefix + part_name);
+    result.push_back(ps);
+    joint = joint->NextSiblingElement("joint");
+  }
+
+  return result;
+}
+
+class AssembleStateServer: public rclcpp::Node
+{
+public:
+    AssembleStateServer(rclcpp::NodeOptions options) 
+    : Node(SERVICE_NAME, options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true))
+    {
+      assemble_dir_ = get_parameter(ASSEMBLE_DIR_PARAM_NAME).as_string();
+      assemble_prefix_ = get_parameter(ASSEMBLE_PREFIX_PARAM_NAME).as_string();
+      tf_buffer_ = std::make_unique<tf2_ros::Buffer>(get_clock());
+      tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+      service_ = create_service<rbs_skill_interfaces::srv::AssembleState>(
+        SERVICE_NAME, std::bind(&AssembleStateServer::callback, this, 
+          std::placeholders::_1, std::placeholders::_2));
+    }
+
+    void callback(
+      std::shared_ptr<rbs_skill_interfaces::srv::AssembleState::Request> request,
+      std::shared_ptr<rbs_skill_interfaces::srv::AssembleState::Response> response)
+    {
+      auto state = static_cast<AssembleReqState>(request->req_kind);
+      bool call_status = false;
+      std::string assemble_name = request->assemble_name;
+      std::string part_name = request->part_name;
+      auto assemble = assembles_.find(request->assemble_name);
+      if (assemble == assembles_.end())
+        assembles_.insert(std::make_pair(request->assemble_name, Assemble {
+          .part=request->part_name,
+          .ws=request->workspace,
+          .state=NONE,
+          .poses=get_assemble_poses(assemble_name, part_name, assemble_dir_, assemble_prefix_)
+        }));
+      RCLCPP_INFO(get_logger(), "callback call with assemble name: %s", assemble_name.c_str());
+      switch(state)
+      {
+        case NONE:
+          call_status = false;
+          break;
+        case INITIALIZE:
+          call_status = (assembles_.at(assemble_name).state == NONE) && 
+            on_initialize(&assembles_.at(assemble_name));
+          break;
+        case VALIDATE:
+          response->validate_status = (call_status = (assembles_.at(assemble_name).state == INITIALIZE)) && 
+            on_validate(&assembles_.at(assemble_name));
+          break;
+        case COMPLETE:
+          call_status = (assembles_.at(assemble_name).state == VALIDATE) && 
+            on_complete(&assembles_.at(assemble_name));
+          if (call_status)
+            assembles_.erase(assemble_name);
+          break;
+      }
+
+      response->call_status = call_status;
+      response->assemble_name = assemble_name;
+    }
+   
+private:
+    enum AssembleReqState
+    {
+      NONE=-1,
+      INITIALIZE=0,
+      VALIDATE=1,
+      COMPLETE=2
+    };
+
+    struct Assemble
+    {
+      std::string part;
+      std::string ws;
+      AssembleReqState state;
+      std::vector<geometry_msgs::msg::PoseStamped> poses;
+      std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tf2_broadcaster;
+    };
+
+    bool on_initialize(Assemble* assemble)
+    {
+      RCLCPP_INFO(get_logger(), "initialize assemble state for part: %s", assemble->part.c_str());
+      try
+      {
+        assemble->tf2_broadcaster = std::make_unique<tf2_ros::StaticTransformBroadcaster>(this);
+      } catch (const tf2::TransformException &ex){
+        RCLCPP_WARN(get_logger(), "error while create tf2_broadcaster: %s", ex.what());
+        return false;
+      }
+      assemble->state = INITIALIZE;
+      for (const auto& it: assemble->poses)
+      {
+        geometry_msgs::msg::TransformStamped t;
+        t.header.frame_id = assemble->ws;
+        t.child_frame_id = it.header.frame_id;
+        auto pose = it.pose;
+        t.transform.translation.x = pose.position.x;
+        t.transform.translation.y = pose.position.y;
+        t.transform.translation.z = pose.position.z;
+        t.transform.rotation.x = pose.orientation.x;
+        t.transform.rotation.y = pose.orientation.y;
+        t.transform.rotation.z = pose.orientation.z;
+        t.transform.rotation.w = pose.orientation.w;
+        assemble->tf2_broadcaster->sendTransform(t);
+      }
+
+      return true;
+    }
+
+    bool on_validate(Assemble* assemble)
+    {
+      RCLCPP_INFO(get_logger(), "validate assemble state for part: %s", assemble->part.c_str());
+      std::string frame_from = assemble_prefix_ + assemble->part;
+      std::string frame_to = assemble->part;
+      geometry_msgs::msg::TransformStamped ts;
+      try
+      {
+        ts = tf_buffer_->lookupTransform(frame_to, frame_from, tf2::TimePointZero);
+      } catch (const tf2::TransformException &ex) {
+        return false;
+      }
+      auto pos_validate = ts.transform.translation.x < ASSEMBLE_POSITION_OFFSET &&
+			  ts.transform.translation.y < ASSEMBLE_POSITION_OFFSET &&
+			  ts.transform.translation.z < ASSEMBLE_POSITION_OFFSET;
+		  auto orient_validate = ts.transform.rotation.x < ASSEMBLE_ORIENTATION_OFFSET &&
+			  ts.transform.rotation.y < ASSEMBLE_ORIENTATION_OFFSET &&
+			  ts.transform.rotation.z < ASSEMBLE_ORIENTATION_OFFSET;
+
+      RCLCPP_INFO(get_logger(), "pos_validate: %d, orient_validate: %d", pos_validate, orient_validate);
+      assemble->state = (pos_validate && orient_validate)? VALIDATE: INITIALIZE;
+
+      return assemble->state == VALIDATE;
+    }
+
+    bool on_complete(Assemble* assemble)
+    {
+      RCLCPP_INFO(get_logger(), "complete assemble state for part: %s", assemble->part.c_str());
+      try 
+      {
+			  assemble->tf2_broadcaster.reset();
+        assemble->tf2_broadcaster = NULL;
+        assemble->poses.clear();
+		  } catch (const std::exception &ex) {
+        RCLCPP_WARN(get_logger(), "something happen on tf2.reset(): %s", ex.what());
+			  return false;
+      }
+
+      assemble->state = COMPLETE;
+
+      return true;
+    }
+
+   
+private:
+    std::map<std::string, Assemble> assembles_; 
+    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+    std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tf2_broadcaster_;
+    std::mutex mt;
+    rclcpp::Service<rbs_skill_interfaces::srv::AssembleState>::SharedPtr service_;
+    std::string assemble_dir_;
+    std::string assemble_prefix_;
+};
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(AssembleStateServer);
--- a/rbs_skill_servers/src/pick_place_pose_loader.cpp
+++ b/rbs_skill_servers/src/pick_place_pose_loader.cpp
@ -24,7 +24,7 @@ void
 GetGraspPlacePoseServer::handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request, 
            rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response)
 {
-    std::string rrr = request->object_name + "_place"; // TODO: replace with better name
+    std::string rrr = "ASSEMBLE_" + request->object_name; // TODO: replace with better name
    try {
        tf_data = tf_buffer_->lookupTransform(
        "world", rrr.c_str(),
--- a/rbs_task_planner/README.md
+++ b/rbs_task_planner/README.md
@ -0,0 +1,35 @@
+### Usage
+change `rbs_simulation.laucnh.py` file by providing `${PATH}` and `${PREFIX}` to assemble files
+```python
+    assemble_state = Node(
+        package="rbs_skill_servers",
+        executable="assemble_state_service_server",
+        output="screen",
+        emulate_tty=True,
+        parameters=[
+            {"assemble_prefix": "ASSEMBLE_"} # ${PREFIX}
+            {"assemble_package_dir": ${COLCON_WS}/src/robossembler-ros2/rbs_task_planner/exemples/sdf_models}, # ${PATH}
+        ]
+    )
+```
+launch simulation proccess
+```bash
+ros2 launch rbs_simulation rbs_simulation.launch.py
+```
+launch task_planner process
+```bash
+ros2 launch rbs_task_planner task_planner.launch.py
+```
+start env_manager process
+```bash
+./${COLCON_WS}/build/env_manager/main
+```
+you can send `problem` throw plansys2 service with `${PROBLEM_FILE_CONTENT}`
+```bash
+ros2 service call /problem_expert/add_problem plansys2_msgs/srv/AddProblem '{problem: ${PROBLEM_FILE_CONTENT}}'
+```
+or you can use `plansys2_terminal`
+```bash
+ros2 run plansys2_terminal plansys2_terminal
+```
+after that use `plansys2_terminal` to call `run` command and start executing process
--- a/rbs_task_planner/config/params.yaml
+++ b/rbs_task_planner/config/params.yaml
@ -0,0 +1,10 @@
+assemble:
+  ros__parameters:
+    plugins:
+      - rbs_skill_move_topose_bt_action_client
+      - rbs_skill_get_pick_place_pose_service_client
+      - rbs_skill_gripper_move_bt_action_client 
+      - rbs_skill_move_joint_state
+      - rbs_assemble_process_state  
+      - rbs_detect_object
+
--- a/rbs_task_planner/domain/atomic_domain.pddl
+++ b/rbs_task_planner/domain/atomic_domain.pddl
@ -1,7 +1,7 @@
 (define (domain atomic_domain)
    (:requirements :strips :typing :adl :fluents :durative-actions)
    (:types
-        workspace - zone
+        zone
        part
        arm
        assembly
@ -19,16 +19,17 @@
        :parameters (?p - part ?prev ?next - assembly ?z - zone ?a - arm)
        :duration ( = ?duration 1)
        :condition (and 
-            (at start (assembly_order ?prev ?next))
-            (at start (assembled ?prev))
-            (at start (assembly_at ?prev ?z))
-            (at start (part_of ?p ?next))
            (at start (arm_available ?a))
+            (at start (assembly_order ?prev ?next))
+            (at start (part_of ?p ?next))
+            (at start (assembly_at ?prev ?z))
+            (at start (assembled ?prev))
        )
        :effect (and
            (at start (not (arm_available ?a)))
            (at end (assembly_at ?next ?z))
            (at end (assembled ?next))
+            (at end (arm_available ?a))
        )
    )
 )
--- a/rbs_task_planner/example/sdf_models/asm1.sdf
+++ b/rbs_task_planner/example/sdf_models/asm1.sdf
@ -0,0 +1,19 @@
+<?xml version='1.0' encoding='utf-8'?>
+<sdf version="1.7">
+<model name="assembly">
+  <link name="base_link">
+    <pose>0 0 0 0 0 0</pose>
+  </link>
+  <include>
+    <uri>model://box1</uri>
+    <name>box1</name>
+    <pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <joint name="box1" type="fixed">
+    <parent>base_link</parent>
+    <child>box1</child>
+    <pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+</model>
+</sdf>
+
--- a/rbs_task_planner/example/sdf_models/asm2.sdf
+++ b/rbs_task_planner/example/sdf_models/asm2.sdf
@ -0,0 +1,28 @@
+<?xml version='1.0' encoding='utf-8'?>
+<sdf version="1.7">
+<model name="assembly">
+  <link name="base_link">
+    <pose>0 0 0 0 0 0</pose>
+  </link>
+  <include>
+    <uri>model://box1</uri>
+    <name>box1</name>
+    <pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box2</name>
+    <pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <joint name="box1" type="fixed">
+    <parent>base_link</parent>
+    <child>box1</child>
+    <pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box2" type="fixed">
+    <parent>base_link</parent>
+    <child>box2</child>
+    <pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+</model>
+</sdf>
--- a/rbs_task_planner/example/sdf_models/asm3.sdf
+++ b/rbs_task_planner/example/sdf_models/asm3.sdf
@ -0,0 +1,38 @@
+<?xml version='1.0' encoding='utf-8'?>
+<sdf version="1.7">
+<model name="assembly">
+  <link name="base_link">
+    <pose>0 0 0 0 0 0</pose>
+  </link>
+  <include>
+    <uri>model://box1</uri>
+    <name>box1</name>
+    <pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box2</name>
+    <pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box3</name>
+    <pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <joint name="box1" type="fixed">
+    <parent>base_link</parent>
+    <child>box1</child>
+    <pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box2" type="fixed">
+    <parent>base_link</parent>
+    <child>box2</child>
+    <pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box3" type="fixed">
+    <parent>base_link</parent>
+    <child>box3</child>
+    <pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+</model>
+</sdf>
--- a/rbs_task_planner/example/sdf_models/asm4.sdf
+++ b/rbs_task_planner/example/sdf_models/asm4.sdf
@ -0,0 +1,48 @@
+<?xml version='1.0' encoding='utf-8'?>
+<sdf version="1.7">
+<model name="assembly">
+  <link name="base_link">
+    <pose>0 0 0 0 0 0</pose>
+  </link>
+  <include>
+    <uri>model://box1</uri>
+    <name>box1</name>
+    <pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box2</name>
+    <pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box3</name>
+    <pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box4</name>
+    <pose>-0.45 0.0275 0.49 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <joint name="box1" type="fixed">
+    <parent>base_link</parent>
+    <child>box1</child>
+    <pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box2" type="fixed">
+    <parent>base_link</parent>
+    <child>box2</child>
+    <pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box3" type="fixed">
+    <parent>base_link</parent>
+    <child>box3</child>
+    <pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box4" type="fixed">
+    <parent>base_link</parent>
+    <child>box4</child>
+    <pose>-0.45 0.0275 0.49 -3.1415927 0.0 0.0</pose> 
+  </joint>
+</model>
+</sdf>
--- a/rbs_task_planner/example/sdf_models/asm5.sdf
+++ b/rbs_task_planner/example/sdf_models/asm5.sdf
@ -0,0 +1,58 @@
+<?xml version='1.0' encoding='utf-8'?>
+<sdf version="1.7">
+<model name="assembly">
+  <link name="base_link">
+    <pose>0 0 0 0 0 0</pose>
+  </link>
+  <include>
+    <uri>model://box1</uri>
+    <name>box1</name>
+    <pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box2</name>
+    <pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box3</name>
+    <pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box4</name>
+    <pose>-0.45 0.0275 0.49 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box5</name>
+    <pose>-0.45 -0.0275 0.49 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <joint name="box1" type="fixed">
+    <parent>base_link</parent>
+    <child>box1</child>
+    <pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box2" type="fixed">
+    <parent>base_link</parent>
+    <child>box2</child>
+    <pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box3" type="fixed">
+    <parent>base_link</parent>
+    <child>box3</child>
+    <pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box4" type="fixed">
+    <parent>base_link</parent>
+    <child>box4</child>
+    <pose>-0.45 0.0275 0.49 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box5" type="fixed">
+    <parent>base_link</parent>
+    <child>box5</child>
+    <pose>-0.45 -0.0275 0.49 -3.1415927 0.0 0.0</pose> 
+  </joint>
+</model>
+</sdf>
--- a/rbs_task_planner/example/sdf_models/asm6.sdf
+++ b/rbs_task_planner/example/sdf_models/asm6.sdf
@ -0,0 +1,68 @@
+<?xml version='1.0' encoding='utf-8'?>
+<sdf version="1.7">
+<model name="assembly">
+  <link name="base_link">
+    <pose>0 0 0 0 0 0</pose>
+  </link>
+  <include>
+    <uri>model://box1</uri>
+    <name>box1</name>
+    <pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box2</name>
+    <pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box3</name>
+    <pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box4</name>
+    <pose>-0.45 0.0275 0.49 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box5</name>
+    <pose>-0.45 -0.0275 0.49 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <include>
+    <uri>model://box1</uri>
+    <name>box6</name>
+    <pose>-0.45 0.0 0.56 -3.1415927 0.0 0.0</pose> 
+  </include>
+  <joint name="box1" type="fixed">
+    <parent>base_link</parent>
+    <child>box1</child>
+    <pose>-0.45 -0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box2" type="fixed">
+    <parent>base_link</parent>
+    <child>box2</child>
+    <pose>-0.45 0.0 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box3" type="fixed">
+    <parent>base_link</parent>
+    <child>box3</child>
+    <pose>-0.45 0.055 0.46 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box4" type="fixed">
+    <parent>base_link</parent>
+    <child>box4</child>
+    <pose>-0.45 0.0275 0.49 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box5" type="fixed">
+    <parent>base_link</parent>
+    <child>box5</child>
+    <pose>-0.45 -0.0275 0.49 -3.1415927 0.0 0.0</pose> 
+  </joint>
+  <joint name="box6" type="fixed">
+    <parent>base_link</parent>
+    <child>box6</child>
+    <pose>-0.45 0.0 0.56 -3.1415927 0.0 0.0</pose> 
+  </joint>
+</model>
+</sdf>
--- a/rbs_task_planner/example/sdf_models/cat_problem
+++ b/rbs_task_planner/example/sdf_models/cat_problem
@ -0,0 +1 @@
+(define (problem box_problem)(:domain atomic_domain)(:objects ur_manipulator_gripper - arm world - zone box1 box2 box3 box4 box5 box6 - part asm0 asm1 asm2 asm3 asm4 asm5 asm6 - assembly)(:init (part_of box1 asm1) (part_of box2 asm2) (part_of box3 asm3) (part_of box4 asm4) (part_of box5 asm5) (part_of box6 asm6) (assembly_order asm0 asm1) (assembly_order asm1 asm2) (assembly_order asm2 asm3) (assembly_order asm3 asm4) (assembly_order asm4 asm5) (assembly_order asm5 asm6) (arm_available ur_manipulator_gripper) (assembly_at asm0 world) (assembled asm0) )(:goal (and (assembled asm6))))
--- a/rbs_task_planner/example/sdf_models/problem.pddl
+++ b/rbs_task_planner/example/sdf_models/problem.pddl
@ -0,0 +1,26 @@
+(define (problem box_problem)
+(:domain atomic_domain)
+(:objects
+    ur_manipulator_gripper - arm 
+    workspace1 - zone
+    box1 box2 box3 - part
+    asm0 asm1 asm2 asm3 - assembly
+)
+
+(:init 
+    (part_of box1 asm1) 
+    (part_of box2 asm2)
+    (part_of box3 asm3)
+    (assembly_order asm0 asm1)
+    (assembly_order asm1 asm2)
+    (assembly_order asm2 asm3)
+
+    (arm_available ur_manipulator_gripper)
+    (assembly_at asm0 workspace1)
+    (assembled asm0)
+)
+
+(:goal (and 
+  (assembled asm3) 
+))
+)
--- a/rbs_task_planner/launch/task_planner.launch.py
+++ b/rbs_task_planner/launch/task_planner.launch.py
@ -6,7 +6,7 @@ from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
-# from launch_ros.actions import Node
+from launch_ros.actions import Node


 def load_file(package_name, file_path):
@ -36,6 +36,7 @@ def load_yaml(package_name, file_path):
 def generate_launch_description():

    pkg_dir = get_package_share_directory('rbs_task_planner')
+    bt_exec_dir = get_package_share_directory('rbs_bt_executor')
    namespace = LaunchConfiguration('namespace')

    declare_namespace_cmd = DeclareLaunchArgument(
@ -52,7 +53,6 @@ def generate_launch_description():
          'model_file': pkg_dir + '/domain/atomic_domain.pddl',
          'namespace': namespace
        }.items())
-    """
    assemble = Node(
        package='plansys2_bt_actions',
        executable='bt_action_node',
@ -61,20 +61,16 @@ def generate_launch_description():
        output='screen',
        parameters=[
          pkg_dir + '/config/params.yaml',
-          robot_description,
-          robot_description_semantic,
-          kinematics_yaml,
          {
            'action_name': 'assemble',
-            # 'publisher_port': 1666,
-            # 'server_port': 1667,
-            'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/assemble.xml',
+            'bt_xml_file': bt_exec_dir + '/bt_trees/assemble.xml',
          }
        ])
-    """
    ld = LaunchDescription()
    ld.add_action(declare_namespace_cmd)

    # Declare the launch options
    ld.add_action(plansys2_cmd)
+    ld.add_action(assemble)
+
    return ld
--- a/rbs_task_planner/package.xml
+++ b/rbs_task_planner/package.xml
@ -18,9 +18,12 @@
  <depend>plansys2_planner</depend>
  <depend>plansys2_executor</depend>
  <depend>plansys2_bt_actions</depend>
+  <depend>plansys2_terminal</depend>
  <depend>behaviortree_cpp_v3</depend>
+  <depend>behavior_tree</depend>
 
  <exec_depend>popf</exec_depend>
+  <exec_depend>plansys2_bringup</exec_depend>

  <test_depend>ament_lint_common</test_depend>
  <test_depend>ament_lint_auto</test_depend>
--- a/rbs_task_planner/problems/example_problem.pddl
+++ b/rbs_task_planner/problems/example_problem.pddl
@ -0,0 +1,32 @@
+(define (problem box_problem)
+(:domain atomic_domain)
+(:objects
+    ur_manipulator_gripper - arm 
+    world - zone
+    box1 box2 box3 box4 box5 box6 - part
+    asm0 asm1 asm2 asm3 asm4 asm5 asm6 - assembly
+)
+
+(:init 
+    (part_of box1 asm1) 
+    (part_of box2 asm2)
+    (part_of box3 asm3)
+    (part_of box4 asm4)
+    (part_of box5 asm5)
+    (part_of box6 asm6)
+    (assembly_order asm0 asm1)
+    (assembly_order asm1 asm2)
+    (assembly_order asm2 asm3)
+    (assembly_order asm3 asm4)
+    (assembly_order asm4 asm5)
+    (assembly_order asm5 asm6)
+
+    (arm_available ur_manipulator_gripper)
+    (assembly_at asm0 world)
+    (assembled asm0)
+)
+
+(:goal (and 
+  (assembled asm6) 
+))
+)
--- a/rbs_task_planner/problems/problem.pddl
+++ b/rbs_task_planner/problems/problem.pddl
@ -1,30 +1,31 @@
-(define (problem cube3-problem)
+(define (problem Cube3-problem)
 (:domain atomic_domain)
 (:objects
-    ws1 - zone
-    rbsarm - arm
-    cube3 cube1 cube2 cube004 cube005 cube006 cube007 - part
-    zeroasm subasm0 subasm1 subasm2 subasm3 subasm4 subasm5 subasm6 - assembly
+    ur_manipulator_gripper - arm 
+    ws - zone
+    cube cube001 cube002 cube004 cube005 cube006 - part
+    asm1 asm2 asm3 asm4 asm5 asm6 - assembly
 )

 (:init 
-    (part_of cube3 subasm0) 
-    (part_of cube1 subasm1) 
- 	(part_of cube2 subasm2) 
- 	(part_of cube004 subasm3) 
- 	(part_of cube005 subasm4) 
- 	(part_of cube006 subasm5) 
- 	(part_of cube007 subasm6)
-    (assembly_order zeroasm subasm0)
- 	(assembly_order subasm0 subasm1)
- 	(assembly_order subasm1 subasm2)
- 	(assembly_order subasm3 subasm4)
- 	(assembly_order subasm5 subasm6)
+    (part_of cube asm1) 
+    (part_of cube001 asm2)
+    (part_of cube002 asm3)
+    (part_of cube004 asm4) 
+    (part_of cube005 asm5) 
+    (part_of cube006 asm6) 
+    (assembly_order asm1 asm2)
+    (assembly_order asm2 asm3)
+    (assembly_order asm3 asm4)
+    (assembly_order asm4 asm5)
+    (assembly_order asm5 asm6)

-    (arm_available rbsarm)
-    (assembly_at zeroasm ws1)
-    (assembled zeroasm)
+    (arm_available ur_manipulator_gripper)
+    (assembly_at asm1 ws)
+    (assembled asm1)
 )

-(:goal (and (assembled subasm6)))
+(:goal (and 
+  (assembled asm6) 
+))
 )
				`@ -0,0 +1 @@`
				(define (problem box_problem)(:domain atomic_domain)(:objects ur_manipulator_gripper - arm world - zone box1 box2 box3 box4 box5 box6 - part asm0 asm1 asm2 asm3 asm4 asm5 asm6 - assembly)(:init (part_of box1 asm1) (part_of box2 asm2) (part_of box3 asm3) (part_of box4 asm4) (part_of box5 asm5) (part_of box6 asm6) (assembly_order asm0 asm1) (assembly_order asm1 asm2) (assembly_order asm2 asm3) (assembly_order asm3 asm4) (assembly_order asm4 asm5) (assembly_order asm5 asm6) (arm_available ur_manipulator_gripper) (assembly_at asm0 world) (assembled asm0) )(:goal (and (assembled asm6))))