add 'remove' action to plansys2

robossembler/CMakeLists.txt

@@ -69,6 +69,12 @@ list(APPEND plugin_libs robossembler_print_bt_node)
 add_library(robossembler_get_grasp_poses_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/GetGraspPoses.cpp)
 list(APPEND plugin_libs robossembler_get_grasp_poses_bt_action_client)
 
+add_library(robossembler_get_workspace_placement_pose_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/GetWorkspacePlacementPose.cpp)
+list(APPEND plugin_libs robossembler_get_workspace_placement_pose_bt_action_client)
+
+add_library(robossembler_construct_workspace_placement_pose_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/ConstructWorkspacePlacementPose.cpp)
+list(APPEND plugin_libs robossembler_construct_workspace_placement_pose_bt_action_client)
+
 foreach(bt_plugin ${plugin_libs})
     ament_target_dependencies(${bt_plugin} ${dependencies})
     target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

robossembler/behavior_trees_xml/atomic_skills_xml/remove.xml

@@ -53,6 +53,54 @@
                 server_timeout="10" 
                 cancel_timeout="5" />
 
+            <Action ID="ConstructWorkspacePlacementPose"
+                part_id="${arg0}"
+                workspace="${arg2}"
+                constructed_part="constructed_part"
+
+                server_name="/scene_monitor/construct_workspace_placement_pose"
+                server_timeout="10"/>
+
+            <Action ID="GetWorkspacePlacementPose" 
+                part_id="{constructed_part}" 
+                workspace="${arg2}" 
+                pre_placement_pose="{pre_placement_pose}" 
+                placement_pose="{placement_pose}"
+                
+                server_name="/scene_monitor/get_workspace_placement_pose" 
+                server_timeout="10" />
+
+            <Action ID="MoveToPose"  
+                robot_name="${arg3}" 
+                pose="{pre_placement_pose}"
+
+                server_name="/move_topose" 
+                server_timeout="10" 
+                cancel_timeout="5" />
+            
+            <Action ID="MoveToPose"  
+                robot_name="${arg3}" 
+                pose="{placement_pose}"
+
+                server_name="/move_topose" 
+                server_timeout="10" 
+                cancel_timeout="5" />
+            
+            <Action ID="GripperCmd" 
+                gap_distance="{pre_gap_distance}"
+                frame_name="${arg0}"
+                
+                server_name="/gripper_cmd" 
+                server_timeout="10" 
+                cancel_timeout="5" />
+            
+            <Action ID="MoveToPose"  
+                robot_name="${arg3}" 
+                pose="{pre_placement_pose}"
+
+                server_name="/move_topose" 
+                server_timeout="10" 
+                cancel_timeout="5" />
 
         </Sequence>
     </BehaviorTree>
@@ -77,7 +125,20 @@
             <input_port name="gap_distance"/>
             <input_port name="frame_name"/>
         </Action>
+        
+        <Action ID="ConstructWorkspacePlacementPose">
+            <input_port name="part_id"/>
+            <input_port name="workspace"/>
+            <output_port name="constructed_part"/>
+        </Action>
     
+        <Action ID="GetWorkspacePlacementPose">
+            <input_port name="part_id"/>
+            <input_port name="workspace"/>
+            <output_port name="pre_placement_pose"/>
+            <output_port name="placement_pose"/>
+        </Action>
+
     </TreeNodesModel>
     <!-- ////////// -->
 </root>

robossembler/config/params.yaml

@@ -17,4 +17,6 @@ remove_part:
       - robossembler_get_grasp_poses_bt_action_client
       - robossembler_move_topose_bt_action_client
       - robossembler_move_gripper_bt_action_client
+      - robossembler_get_workspace_placement_pose_bt_action_client
+      - robossembler_construct_workspace_placement_pose_bt_action_client
 

robossembler/include/robot_bt/behavior_tree_nodes/ConstructWorkspacePlacementPose.hpp

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <behavior_tree/BtService.hpp>
+#include <scene_monitor_interfaces/srv/construct_workspace_placement_pose.hpp>
+
+#include <behaviortree_cpp_v3/bt_factory.h>
+#include <geometry_msgs/msg/pose.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+
+class ConstructWorkspacePlacementPose : public BtService<scene_monitor_interfaces::srv::ConstructWorkspacePlacementPose>
+{
+public:
+    ConstructWorkspacePlacementPose(const std::string &xml_tag_name, 
+        const BT::NodeConfiguration &conf);
+
+    scene_monitor_interfaces::srv::ConstructWorkspacePlacementPose::Request::SharedPtr populate_request() override;
+
+    BT::NodeStatus handle_response(scene_monitor_interfaces::srv::ConstructWorkspacePlacementPose::Response::SharedPtr response) override;
+
+    static BT::PortsList providedPorts()
+    {
+        return providedBasicPorts({
+            BT::InputPort<std::string>("part_id"),
+            BT::InputPort<std::string>("workspace"),
+            BT::OutputPort<std::string>("constructed_part"),
+        });
+    }
+
+private:
+    std::string frame_;
+    std::string workspace_;
+};

robossembler/include/robot_bt/behavior_tree_nodes/GetWorkspacePlacementPose.hpp

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <behavior_tree/BtService.hpp>
+#include <scene_monitor_interfaces/srv/get_workspace_placement_pose.hpp>
+
+#include <behaviortree_cpp_v3/bt_factory.h>
+#include <geometry_msgs/msg/pose.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+
+class GetWorkspacePlacementPose : public BtService<scene_monitor_interfaces::srv::GetWorkspacePlacementPose>
+{
+public:
+    GetWorkspacePlacementPose(const std::string &xml_tag_name, 
+        const BT::NodeConfiguration &conf);
+
+    scene_monitor_interfaces::srv::GetWorkspacePlacementPose::Request::SharedPtr populate_request() override;
+
+    BT::NodeStatus handle_response(scene_monitor_interfaces::srv::GetWorkspacePlacementPose::Response::SharedPtr response) override;
+
+    static BT::PortsList providedPorts()
+    {
+        return providedBasicPorts({
+            BT::InputPort<std::string>("part_id"),
+            BT::InputPort<std::string>("workspace"),
+            BT::OutputPort<geometry_msgs::msg::Pose>("placement_pose"),
+            BT::OutputPort<geometry_msgs::msg::Pose>("pre_placement_pose"),
+        });
+    }
+
+private:
+    std::string frame_;
+    std::string workspace_;
+    geometry_msgs::msg::Pose placement_pose_;
+    geometry_msgs::msg::Pose pre_placement_pose_;
+};

robossembler/src/behavior_tree_nodes/atomic_skills/ConstructWorkspacePlacementPose.cpp

@@ -0,0 +1,58 @@
+#include "robot_bt/behavior_tree_nodes/ConstructWorkspacePlacementPose.hpp"
+
+#include <scene_monitor_interfaces/srv/get_grasp_poses.hpp>
+#include <scene_monitor_interfaces/srv/construct_workspace_placement_pose.hpp>
+#include <rclcpp/qos.hpp>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
+#include <iostream>
+#include <filesystem>
+#include <fstream>
+#include <sstream>
+#include <thread>
+
+
+using ConstructWorkspacePlacementPoseSrv = scene_monitor_interfaces::srv::ConstructWorkspacePlacementPose;
+
+ConstructWorkspacePlacementPose::ConstructWorkspacePlacementPose(const std::string &xml_tag_name,
+                const BT::NodeConfiguration &conf)
+:BtService<ConstructWorkspacePlacementPoseSrv>(xml_tag_name, conf)
+{}
+
+ConstructWorkspacePlacementPoseSrv::Request::SharedPtr ConstructWorkspacePlacementPose::populate_request()
+{
+    std::string frame, printer;
+    getInput<std::string>("part_id", frame_);
+    RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< frame_ <<"]");
+    
+    getInput<std::string>("workspace", workspace_);
+    RCLCPP_INFO_STREAM(_node->get_logger(), "Provided printer name ["<< workspace_ <<"]");
+
+    auto request = std::make_shared<ConstructWorkspacePlacementPoseSrv::Request>();
+    request->frame_id = frame_;
+    request->workspace_id = workspace_;
+    
+    return request;
+}
+
+BT::NodeStatus ConstructWorkspacePlacementPose::handle_response(ConstructWorkspacePlacementPoseSrv::Response::SharedPtr response)
+{
+    RCLCPP_INFO(_node->get_logger(), "Handle Response call");
+    
+    if (!response->success)
+    {
+        RCLCPP_WARN(_node->get_logger(), "Get failure response from service with msg: `%s`", response->message.c_str());
+        return BT::NodeStatus::FAILURE;
+    }
+
+    RCLCPP_INFO(_node->get_logger(), "Get success response status with msg: `%s`", response->message.c_str());
+
+    setOutput<std::string>("constructed_part", frame_ + "_" + workspace_);
+
+    return BT::NodeStatus::SUCCESS;
+}
+
+BT_REGISTER_NODES(factory)
+{
+    factory.registerNodeType<ConstructWorkspacePlacementPose>("ConstructWorkspacePlacementPose");
+}

robossembler/src/behavior_tree_nodes/atomic_skills/GetWorkspacePlacementPose.cpp

@@ -0,0 +1,61 @@
+#include "robot_bt/behavior_tree_nodes/GetWorkspacePlacementPose.hpp"
+
+#include <scene_monitor_interfaces/srv/get_workspace_placement_pose.hpp>
+#include <rclcpp/qos.hpp>
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
+#include <iostream>
+#include <filesystem>
+#include <fstream>
+#include <sstream>
+#include <thread>
+
+
+using GetWorkspacePlacementPoseSrv = scene_monitor_interfaces::srv::GetWorkspacePlacementPose;
+
+GetWorkspacePlacementPose::GetWorkspacePlacementPose(const std::string &xml_tag_name,
+                const BT::NodeConfiguration &conf)
+:BtService<GetWorkspacePlacementPoseSrv>(xml_tag_name, conf)
+{}
+
+GetWorkspacePlacementPoseSrv::Request::SharedPtr GetWorkspacePlacementPose::populate_request()
+{
+    std::string frame, printer;
+    getInput<std::string>("part_id", frame_);
+    RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< frame_ <<"]");
+    
+    getInput<std::string>("workspace", workspace_);
+    RCLCPP_INFO_STREAM(_node->get_logger(), "Provided printer name ["<< workspace_ <<"]");
+
+    auto request = std::make_shared<GetWorkspacePlacementPoseSrv::Request>();
+    request->frame_id = frame_;
+    request->workspace_id = workspace_;
+    
+    return request;
+}
+
+BT::NodeStatus GetWorkspacePlacementPose::handle_response(GetWorkspacePlacementPoseSrv::Response::SharedPtr response)
+{
+    RCLCPP_INFO(_node->get_logger(), "Handle Response call");
+    
+    if (!response->success)
+    {
+        RCLCPP_WARN(_node->get_logger(), "Get failure response from service with msg: `%s`", response->message.c_str());
+        return BT::NodeStatus::FAILURE;
+    }
+
+    RCLCPP_INFO(_node->get_logger(), "Get success response status with msg: `%s`", response->message.c_str());
+
+    scene_monitor_interfaces::msg::GraspPose pose = response->grasp_poses[0];
+    pose.grasp_pose.position.z += 0.015;
+    setOutput<geometry_msgs::msg::Pose>("placement_pose", pose.grasp_pose);
+    pose.pre_grasp_pose.position.z += 0.015;
+    setOutput<geometry_msgs::msg::Pose>("pre_placement_pose", pose.pre_grasp_pose);
+
+    return BT::NodeStatus::SUCCESS;
+}
+
+BT_REGISTER_NODES(factory)
+{
+    factory.registerNodeType<GetWorkspacePlacementPose>("GetWorkspacePlacementPose");
+}

robossembler_scene_monitor/scene_monitor/config/component_state_node.param.yaml

@@ -1,11 +1,11 @@
-component_state_monitor_node:
+component_state_monitor:
   ros__parameters:
     models-package: "sdf_models"
     global_frame: "world"
     printers_names: ["printer1", "printer2"]
     printers:
       printer1:
-        position: [0.3, 0.3, 0.012]
+        position: [0.5, 0.0, 0.012]
         orientation: [0.0, 0.0, 0.0, 1.0]
       printer2:
         position: [0.0, 0.5, 0.012]
@@ -13,8 +13,8 @@ component_state_monitor_node:
     workspaces_names: ["workspace1", "workspace2"]
     workspaces:
       workspace1:
-        position: [-0.5, 0.0, 0.012]
-        orientation: [0.0, 0.0, 0.0, 0.0]
+        position: [0.3, 0.4, 0.012]
+        orientation: [0.0, 0.0, 0.5, 1.0]
       workspace2:
         position: [0.0, -0.5, 0.012]
         orientation: [0.0, 0.0, 0.0, 0.0]

robossembler_scene_monitor/scene_monitor/include/component.hpp

@@ -49,8 +49,12 @@ public:
     scene_monitor_interfaces::msg::GraspPose getGraspPose(const std::string &grasp_pose_name) const;
     std::vector<scene_monitor_interfaces::msg::GraspPose> getGraspPoses() const;
     geometry_msgs::msg::Pose getPlacementPose() const;
+    bool getIsImage() const;
 
     void setFrameName(const std::string &frame_name);
+    void setGraspPoses(const std::vector<scene_monitor_interfaces::msg::GraspPose> &grasp_poses);
+    void setPlacementPose(const geometry_msgs::msg::Pose &pose);
+    void setIsImage(const bool &is_image);
     
     void initializeFromJson(json &input);
 
@@ -65,6 +69,8 @@ private:
     std::map<std::string, scene_monitor_interfaces::msg::GraspPose> _grasp_poses;
     std::vector<std::string> _component_links;
 
+    bool _is_image;
+
     geometry_msgs::msg::Pose _current_pose;
 };
 

robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp

@@ -14,7 +14,8 @@
 #include "scene_monitor_interfaces/srv/get_grasp_pose.hpp"
 #include "scene_monitor_interfaces/srv/get_grasp_poses.hpp"
 #include "scene_monitor_interfaces/srv/print_part.hpp"
-#include "scene_monitor_interfaces/srv/get_workspace_pose.hpp"
+#include "scene_monitor_interfaces/srv/get_workspace_placement_pose.hpp"
+#include "scene_monitor_interfaces/srv/construct_workspace_placement_pose.hpp"
 
 #include "component.hpp"
 #include "component_spawner.hpp"
@@ -27,7 +28,8 @@ using GetPartSrv = scene_monitor_interfaces::srv::GetPart;
 using GetGraspPoseSrv = scene_monitor_interfaces::srv::GetGraspPose;
 using GetGraspPosesSrv = scene_monitor_interfaces::srv::GetGraspPoses;
 using PrintPartSrv = scene_monitor_interfaces::srv::PrintPart;
-using GetWorkspacePoseSrv = scene_monitor_interfaces::srv::GetWorkspacePose;
+using GetWorkspacePlacementPoseSrv = scene_monitor_interfaces::srv::GetWorkspacePlacementPose;
+using ConstructWorkspacePlacementPoseSrv = scene_monitor_interfaces::srv::ConstructWorkspacePlacementPose;
 
 class ComponentStateMonitor: public rclcpp::Node
 {
@@ -41,16 +43,18 @@ public:
 
 private:
 
-    void getComponent(const std::shared_ptr<GetPartSrv::Request> request,
-        std::shared_ptr<GetPartSrv::Response> response);
-    void getGraspPoses(const std::shared_ptr<GetGraspPosesSrv::Request> request,
-        std::shared_ptr<GetGraspPosesSrv::Response> response);
-    void getGraspPose(const std::shared_ptr<GetGraspPoseSrv::Request> request,
-        std::shared_ptr<GetGraspPoseSrv::Response> response);
-    void printComponent(const std::shared_ptr<PrintPartSrv::Request> request,
-        std::shared_ptr<PrintPartSrv::Response> response);
-    void getWorkspacePose(const std::shared_ptr<GetWorkspacePoseSrv::Request> request,
-        std::shared_ptr<GetWorkspacePoseSrv::Response> response);
+    void getComponent(const GetPartSrv::Request::SharedPtr request,
+        GetPartSrv::Response::SharedPtr response);
+    void getGraspPoses(const GetGraspPosesSrv::Request::SharedPtr request,
+        GetGraspPosesSrv::Response::SharedPtr response);
+    void getGraspPose(const GetGraspPoseSrv::Request::SharedPtr request,
+        GetGraspPoseSrv::Response::SharedPtr response);
+    void printComponent(const PrintPartSrv::Request::SharedPtr request,
+        PrintPartSrv::Response::SharedPtr response);
+    void getWorkspacePlacementPose(const GetWorkspacePlacementPoseSrv::Request::SharedPtr request,
+        GetWorkspacePlacementPoseSrv::Response::SharedPtr response);
+    void constructWorkspacePlacementPose(const ConstructWorkspacePlacementPoseSrv::Request::SharedPtr request,
+        ConstructWorkspacePlacementPoseSrv::Response::SharedPtr response);
 
     void makeTransform(const Component& component, const std::string& parent);
     void makeTransform(const std::string& frame_name, const geometry_msgs::msg::Pose& frame_pose, 
@@ -58,11 +62,13 @@ private:
     void updateComponents();
 
     std::map<std::string, Component> _loaded_components;
+    std::map<std::string, Component> _imaginary_components; 
     rclcpp::Service<GetPartSrv>::SharedPtr _get_part_service;
     rclcpp::Service<GetGraspPoseSrv>::SharedPtr _get_grasp_pose_service;
     rclcpp::Service<GetGraspPosesSrv>::SharedPtr _get_grasp_poses_service;
     rclcpp::Service<PrintPartSrv>::SharedPtr _print_part_service;
-    rclcpp::Service<GetWorkspacePoseSrv>::SharedPtr _get_workspace_service;
+    rclcpp::Service<GetWorkspacePlacementPoseSrv>::SharedPtr _get_workspace_placement_service;
+    rclcpp::Service<ConstructWorkspacePlacementPoseSrv>::SharedPtr _construct_workspace_placement_pose;
 
     std::string _models_package;
     std::map<std::string, geometry_msgs::msg::Pose> _loaded_printers;

robossembler_scene_monitor/scene_monitor/launch/scene_monitor.launch.py

@@ -1,26 +1,41 @@
 import os
 from ament_index_python.packages import get_package_share_directory
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
 def generate_launch_description():
-    ld = LaunchDescription()
+
+    namespace = LaunchConfiguration('namespace')
 
     component_state_params = os.path.join(
         get_package_share_directory('robossembler_scene_monitor'),
         'config', 'component_state_node.param.yaml')
 
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
+
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Namespace')
+
     component_state_monitor_node = Node(
         package="robossembler_scene_monitor",
         executable="component_state_monitor_node",
-        name="component_state_monitor_node",
+        name="component_state_monitor",
+        namespace=namespace,
         output="screen",
-        emulate_tty=True,
         parameters=[
             component_state_params
         ]
     )
+
+    ld = LaunchDescription()
     
+    ld.add_action(stdout_linebuf_envvar)
+    ld.add_action(declare_namespace_cmd)
     ld.add_action(component_state_monitor_node)
 
     return ld

robossembler_scene_monitor/scene_monitor/src/component.cpp

@@ -59,6 +59,31 @@ namespace component_state_monitor
         _frame_name = frame_name;
     }
 
+    void Component::setGraspPoses(const std::vector<scene_monitor_interfaces::msg::GraspPose> &grasp_poses)
+    {
+        _grasp_poses.clear();
+
+        for(const auto grasp_pose: grasp_poses)
+        {
+            _grasp_poses.insert({grasp_pose.label, grasp_pose});
+        }
+    }
+
+    void Component::setPlacementPose(const geometry_msgs::msg::Pose &pose)
+    {
+        _placement = pose;
+    }
+
+    void Component::setIsImage(const bool &is_image)
+    {
+        _is_image = is_image;
+    }
+
+    bool Component::getIsImage() const
+    {
+        return _is_image;
+    }
+
     geometry_msgs::msg::Pose Component::getPlacementPose() const
     {
         return _placement;
@@ -137,6 +162,7 @@ namespace component_state_monitor
         }
 
         _current_state = ComponentState::Initialized;
+        _is_image = false;
     }
 
 }

robossembler_scene_monitor/scene_monitor/src/component_pose_estimation.cpp

@@ -25,7 +25,7 @@ geometry_msgs::msg::Pose ComponentPoseEstimation::framePose(const std::string& f
 {
     auto request = std::make_shared<GetEntityStateSrv::Request>();
     request->name = frame_id;
-    RCLCPP_INFO(this->get_logger(), "Provided service call with frame name [%s]", request->name.c_str());
+    // RCLCPP_INFO(this->get_logger(), "Provided service call with frame name [%s]", request->name.c_str());
 
     auto future = _client->async_send_request(request);
 
@@ -44,7 +44,7 @@ geometry_msgs::msg::Pose ComponentPoseEstimation::framePose(const std::string& f
 
     _pose = result->state.pose;
     
-    RCLCPP_INFO(this->get_logger(), "Get success response status");
+    // RCLCPP_INFO(this->get_logger(), "Get success response status");
     // RCLCPP_INFO(this->get_logger(), "update frame (%s) with \npose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f", 
     //             frame_id.c_str(), _pose.position.x, _pose.position.y, _pose.position.z,
     //             _pose.orientation.x, _pose.orientation.y, _pose.orientation.z, _pose.orientation.w);

robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp

@@ -37,10 +37,15 @@ namespace component_state_monitor
                                         std::bind(&ComponentStateMonitor::printComponent, this, 
                                         std::placeholders::_1, std::placeholders::_2));
         
-        // RCLCPP_INFO(this->get_logger(), "Initialize [get_workspace_pose] service");
-        // _get_workspace_service = this->create_service<GetWorkspacePoseSrv>("scene_monitor/get_workspace_pose",
-        //                                 std::bind(&ComponentStateMonitor::getWorkspacePose, this,
-        //                                 std::placeholders::_1, std::placeholders::_2));
+        RCLCPP_INFO(this->get_logger(), "Initialize [get_workspace_placement_pose] service");
+        _get_workspace_placement_service = this->create_service<GetWorkspacePlacementPoseSrv>("scene_monitor/get_workspace_placement_pose",
+                                        std::bind(&ComponentStateMonitor::getWorkspacePlacementPose, this,
+                                        std::placeholders::_1, std::placeholders::_2));
+        
+        RCLCPP_INFO(this->get_logger(), "Initialize [construct_workspace_placement_pose] service");
+        _construct_workspace_placement_pose = this->create_service<ConstructWorkspacePlacementPoseSrv>("scene_monitor/construct_workspace_placement_pose",
+                                        std::bind(&ComponentStateMonitor::constructWorkspacePlacementPose, this,
+                                        std::placeholders::_1, std::placeholders::_2));
         
         _tf2_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
 
@@ -55,22 +60,58 @@ namespace component_state_monitor
         );
     }
 
+    inline geometry_msgs::msg::Pose transformToPose(const geometry_msgs::msg::TransformStamped transformStamped)
+    {
+        geometry_msgs::msg::Pose pose;
+
+        pose.position.x = transformStamped.transform.translation.x;
+        pose.position.y = transformStamped.transform.translation.y;
+        pose.position.z = transformStamped.transform.translation.z;
+
+        pose.orientation.x = transformStamped.transform.rotation.x;
+        pose.orientation.y = transformStamped.transform.rotation.y;
+        pose.orientation.z = transformStamped.transform.rotation.z;
+        pose.orientation.w = transformStamped.transform.rotation.w;
+
+        return pose;
+    }
+
     void ComponentStateMonitor::updateComponents()
     {
         for (const auto& component: _loaded_components)
         {
             if (component.second.getCurrentState() == ComponentState::Printed)
             {
-                makeTransform(component.second, _global_frame);
-                if (component.second.getType() == ComponentType::Assembly)
+                // if (component.second.getIsImage() == true)
+                // {
+                //     makeTransform(component.first, component.second.getPlacementPose(), _global_frame);
+                // } else {
+                    
+                    makeTransform(component.second, _global_frame);
+                // }
+                // if (component.second.getType() == ComponentType::Assembly)
+                // {
+                //     auto links = component.second.getComponentLinks();
+                //     for (const auto link: links)
+                //     {
+                //         auto pose = _pose_estimation_node.framePose(component.first + "::" + link);
+                //         makeTransform(link, pose, _global_frame);
+                //     }
+                // }
+                auto grasp_poses = component.second.getGraspPoses();
+                for (const auto& grasp_pose: grasp_poses)
                 {
-                    auto links = component.second.getComponentLinks();
-                    for (const auto link: links)
-                    {
-                        auto pose = _pose_estimation_node.framePose(component.first + "::" + link);
-                        makeTransform(link, pose, _global_frame);
-                    }
+                    makeTransform(grasp_pose.label, grasp_pose.grasp_pose, component.first);
+                    makeTransform(std::string("pre_" + grasp_pose.label), grasp_pose.pre_grasp_pose, grasp_pose.label);
                 }
+            }
+        }
+
+        if (!_imaginary_components.empty())
+        {
+            for (const auto& component: _imaginary_components)
+            {
+                makeTransform(component.first, component.second.getPlacementPose(), _global_frame);
                 auto grasp_poses = component.second.getGraspPoses();
                 for (const auto& grasp_pose: grasp_poses)
                 {
@@ -201,8 +242,8 @@ namespace component_state_monitor
 
     ComponentStateMonitor::~ComponentStateMonitor() {}
 
-    void ComponentStateMonitor::getComponent(const std::shared_ptr<GetPartSrv::Request> request,
-            std::shared_ptr<GetPartSrv::Response> response)
+    void ComponentStateMonitor::getComponent(const GetPartSrv::Request::SharedPtr request,
+            GetPartSrv::Response::SharedPtr response)
     {
         RCLCPP_INFO(this->get_logger(), "[get_part] Incoming request");
 
@@ -225,48 +266,119 @@ namespace component_state_monitor
         RCLCPP_INFO(this->get_logger(), "[get_part] Sending back response");
     }
 
-    void ComponentStateMonitor::getWorkspacePose(const std::shared_ptr<GetWorkspacePoseSrv::Request> request,
-            std::shared_ptr<GetWorkspacePoseSrv::Response> response)
+    void ComponentStateMonitor::constructWorkspacePlacementPose(const ConstructWorkspacePlacementPoseSrv::Request::SharedPtr request,
+            ConstructWorkspacePlacementPoseSrv::Response::SharedPtr response)
     {
-        RCLCPP_INFO(this->get_logger(), "[get_workspace_pose] Incoming request");
+        RCLCPP_INFO(this->get_logger(), "[construct_workspace_placement_pose] Incoming request");
 
         if (_loaded_workspaces.find(request->workspace_id) != _loaded_workspaces.end())
         {
-            RCLCPP_INFO(this->get_logger(), "[get_workspace_pose] found available workspace (%s) in component_state_monitor", request->workspace_id.c_str());
+            RCLCPP_INFO(this->get_logger(), "[construct_workspace_placement_pose] found available workspace (%s) in component_state_monitor", request->workspace_id.c_str());
             auto workspace = _loaded_workspaces.at(request->workspace_id);
+            
+            if (_loaded_components.find(request->frame_id) != _loaded_components.end())
+            {
+                RCLCPP_INFO(this->get_logger(), "[construct_workspace_placement_pose] found frame (%s) in component_state_monitor", request->frame_id.c_str());
+                auto component = _loaded_components.at(request->frame_id);
+                auto component_frame_name = component.getFrameName()+"_"+request->workspace_id;
 
-            response->workspace_pose = workspace;
-            response->success = true;
-            response->message = "successfully received a response from the service [get_workspace_pose] with workspace (" + request->workspace_id + ")";
+                component.setFrameName(component_frame_name);
+
+                auto frame_pose = component.getPlacementPose();
+                frame_pose.position.x += workspace.position.x;
+                frame_pose.position.y += workspace.position.y;
+                frame_pose.position.z += workspace.position.z;
+
+                frame_pose.orientation.x += workspace.orientation.x;
+                frame_pose.orientation.y += workspace.orientation.y;
+                frame_pose.orientation.z += workspace.orientation.z;
+                frame_pose.orientation.w += workspace.orientation.w;
+
+                component.setPlacementPose(frame_pose);
+
+                auto grasp_poses = component.getGraspPoses();
+                for (auto& grasp_pose: grasp_poses)
+                {
+                    grasp_pose.label += "_" + request->workspace_id;
+                }
+
+                component.setGraspPoses(grasp_poses);
+                component.updateState(ComponentState::Printed);
+                component.setIsImage(true);
+
+                _imaginary_components.insert({component.getFrameName(), component});
+
+                response->success = true;
+                response->message = "successfully received a response from the service [construct_workspace_placement_pose] with workspace (" + request->workspace_id + ")";
+
+            } else {
+                RCLCPP_WARN(this->get_logger(), "[construct_workspace_placement_pose] frame with name (%s) not loaded ", request->frame_id.c_str());
+                response->success = false;
+                response->message = "frame with name (" + request->frame_id + ") not loaded";
+            }
         } else {
 
-            RCLCPP_WARN(this->get_logger(), "[get_workspace_pose] workspaces with name (%s) not available now", request->workspace_id.c_str());
+            RCLCPP_WARN(this->get_logger(), "[construct_workspace_placement_pose] workspaces with name (%s) not available now", request->workspace_id.c_str());
             response->success = false;
             response->message = "workspace with name (" + request->workspace_id + ") not available";
         }
 
-        RCLCPP_INFO(this->get_logger(), "[get_workspace_pose] Sending back response");
+        RCLCPP_INFO(this->get_logger(), "[construct_workspace_placement_pose] Sending back response");
     }
 
-    inline geometry_msgs::msg::Pose transformToPose(const geometry_msgs::msg::TransformStamped transformStamped)
+    void ComponentStateMonitor::getWorkspacePlacementPose(const GetWorkspacePlacementPoseSrv::Request::SharedPtr request,
+            GetWorkspacePlacementPoseSrv::Response::SharedPtr response)
     {
-        geometry_msgs::msg::Pose pose;
+        RCLCPP_INFO(this->get_logger(), "[get_workspace_placement_pose] Incoming request");
 
-        pose.position.x = transformStamped.transform.translation.x;
-        pose.position.y = transformStamped.transform.translation.y;
-        pose.position.z = transformStamped.transform.translation.z;
+        if (_loaded_workspaces.find(request->workspace_id) != _loaded_workspaces.end())
+        {
+            RCLCPP_INFO(this->get_logger(), "[get_workspace_placement_pose] found available workspace (%s) in component_state_monitor", request->workspace_id.c_str());
+            // auto workspace = _loaded_workspaces.at(request->workspace_id);
+            
+            if (_imaginary_components.find(request->frame_id) != _imaginary_components.end())
+            {
+                auto component = _imaginary_components.at(request->frame_id);
+                RCLCPP_INFO(this->get_logger(), "[get_workspace_placement_pose] found frame (%s) in component_state_monitor", request->frame_id.c_str());
 
-        pose.orientation.x = transformStamped.transform.rotation.x;
-        pose.orientation.y = transformStamped.transform.rotation.y;
-        pose.orientation.z = transformStamped.transform.rotation.z;
-        pose.orientation.w = transformStamped.transform.rotation.w;
+                auto grasp_poses = component.getGraspPoses();
+                std::vector<scene_monitor_interfaces::msg::GraspPose> result_poses;
 
-        return pose;
+                for (auto pose: grasp_poses)
+                {
+                    geometry_msgs::msg::TransformStamped transformStamped = _tf2_buffer->lookupTransform(
+                        _global_frame, pose.label, tf2::TimePointZero 
+                    );
+                    pose.grasp_pose = transformToPose(transformStamped);
+
+                    geometry_msgs::msg::TransformStamped preTransformStamped = _tf2_buffer->lookupTransform(
+                        _global_frame, std::string("pre_"+pose.label), tf2::TimePointZero 
+                    );
+                    pose.pre_grasp_pose = transformToPose(preTransformStamped);
+
+                    result_poses.push_back(pose);
+                }
+
+                response->grasp_poses = result_poses;
+                response->success = true;
+                response->message = "successfully received a response from the service [get_workspace_placement_pose] with workspace (" + request->workspace_id + ")";
+            } else {
+                RCLCPP_WARN(this->get_logger(), "[get_workspace_placement_pose] frame with name (%s) not loaded ", request->frame_id.c_str());
+                response->success = false;
+                response->message = "frame with name (" + request->frame_id + ") not loaded";
+            }
+        } else {
+
+            RCLCPP_WARN(this->get_logger(), "[get_workspace_placement_pose] workspaces with name (%s) not available now", request->workspace_id.c_str());
+            response->success = false;
+            response->message = "workspace with name (" + request->workspace_id + ") not available";
+        }
+
+        RCLCPP_INFO(this->get_logger(), "[get_workspace_placement_pose] Sending back response");
     }
 
-
-    void ComponentStateMonitor::getGraspPoses(const std::shared_ptr<GetGraspPosesSrv::Request> request,
-        std::shared_ptr<GetGraspPosesSrv::Response> response)
+    void ComponentStateMonitor::getGraspPoses(const GetGraspPosesSrv::Request::SharedPtr request,
+        GetGraspPosesSrv::Response::SharedPtr response)
     {
         RCLCPP_INFO(this->get_logger(), "[get_grasp_poses] Incoming request");
 
@@ -304,8 +416,8 @@ namespace component_state_monitor
         RCLCPP_INFO(this->get_logger(), "[get_grasp_poses] Sending back response");
     }
 
-    void ComponentStateMonitor::getGraspPose(const std::shared_ptr<GetGraspPoseSrv::Request> request,
-        std::shared_ptr<GetGraspPoseSrv::Response> response)
+    void ComponentStateMonitor::getGraspPose(const GetGraspPoseSrv::Request::SharedPtr request,
+        GetGraspPoseSrv::Response::SharedPtr response)
     {
         RCLCPP_INFO(this->get_logger(), "[get_grasp_pose] Incoming request");
 
@@ -348,8 +460,8 @@ namespace component_state_monitor
         RCLCPP_INFO(this->get_logger(), "[get_grasp_pose] Sending back response");
     }
 
-    void ComponentStateMonitor::printComponent(const std::shared_ptr<PrintPartSrv::Request> request,
-        std::shared_ptr<PrintPartSrv::Response> response)
+    void ComponentStateMonitor::printComponent(const PrintPartSrv::Request::SharedPtr request,
+        PrintPartSrv::Response::SharedPtr response)
     {
         RCLCPP_INFO(this->get_logger(), "[print_part] Incoming request");
 

robossembler_scene_monitor/scene_monitor/src/component_state_monitor_node.cpp

@@ -1,17 +1,16 @@
-#include <rclcpp/rclcpp.hpp>
-// #include "SceneMonitor.hpp"
+#include <memory>
 
 #include "component_state_monitor.hpp"
 
 int main(int argc, char **argv)
 {
     rclcpp::init(argc, argv);
-    rclcpp::executors::MultiThreadedExecutor exec;
+    // rclcpp::executors::MultiThreadedExecutor exec;
 
     auto component_state_monitor_node = std::make_shared<component_state_monitor::ComponentStateMonitor>();
 
-    exec.add_node(component_state_monitor_node);
-    exec.spin();
+    rclcpp::spin(component_state_monitor_node->get_node_base_interface());
+    // exec.spin();
 
     rclcpp::shutdown();
     

robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt

@@ -26,7 +26,8 @@ set(msg_files
 )
 
 set(srv_files
-    "srv/GetWorkspacePose.srv"
+    "srv/ConstructWorkspacePlacementPose.srv"
+    "srv/GetWorkspacePlacementPose.srv"
     "srv/PrintPart.srv"
     "srv/GetGraspPose.srv"
     "srv/GetGraspPoses.srv"

robossembler_scene_monitor/scene_monitor_interfaces/srv/ConstructWorkspacePlacementPose.srv

@@ -0,0 +1,6 @@
+string workspace_id
+string frame_id
+---
+
+bool success   # indicate successful run of triggered service
+string message # informational, e.g. for error messages

robossembler_scene_monitor/scene_monitor_interfaces/srv/GetWorkspacePlacementPose.srv

@@ -1,7 +1,8 @@
 string workspace_id
+string frame_id
 ---
 #result definition
-geometry_msgs/Pose workspace_pose
+GraspPose[] grasp_poses
 
 bool success   # indicate successful run of triggered service
 string message # informational, e.g. for error messages