create remove pddl action

rasms.repos

@@ -10,4 +10,12 @@ repositories:
   plansys2:
     type: git
     url: https://github.com/IntelligentRoboticsLabs/ros2_planning_system
-    version: master
+    version: master
+  behavior_tree:
+    type: git
+    url: https://github.com/berkeleyauv/behavior_tree
+    version: master
+  gazebo_ros_link_attacher:
+    type: git
+    url: https://github.com/davidorchansky/gazebo_ros_link_attacher.git
+    version: foxy-devel

rasmt_support/world/robossembler.world

@@ -69,28 +69,6 @@
         <kinematic>0</kinematic>
       </link>
     </model>
-    <model name="box">
-        <pose>0.5 0 0.1 0 0 0</pose>
-        <link name="link">
-            <collision name="collision">
-                <geometry>
-                <box>
-                    <size>0.15 0.15 0.05</size>
-                </box>
-                </geometry>
-            </collision>
-            <visual name="visual">
-                <geometry>
-                <box>
-                    <size>0.15 0.15 0.05</size>
-                </box>
-                </geometry>
-                <material>
-                <script>Gazebo/WoodPallet</script>
-                </material>
-            </visual>
-        </link>
-    </model>
     <gravity>0 0 -9.8</gravity>
     <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
     <atmosphere type='adiabatic'/>

robossembler/behavior_trees_xml/atomic_skills_xml/remove.xml

@@ -7,24 +7,53 @@
                 part_id="${arg0}" 
                 grasp_pose="{grasp_pose}" 
                 pre_grasp_pose="{pre_grasp_pose}" 
+                pre_gap_distance="{pre_gap_distance}"
                 gap_distance="{gap_distance}" 
                 
                 server_name="/scene_monitor/get_grasp_poses" 
                 server_timeout="10" />
+
             <Action ID="MoveToPose"  
-                arm_group="${arg3}" 
+                robot_name="${arg3}" 
                 pose="{pre_grasp_pose}"
 
                 server_name="/move_topose" 
                 server_timeout="10" 
                 cancel_timeout="5" />
+
             <Action ID="GripperCmd" 
-                gap_distance="{gap_distance}" 
+                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="{grasp_pose}"
+
+                server_name="/move_topose" 
+                server_timeout="10" 
+                cancel_timeout="5" />
+
+            <Action ID="GripperCmd" 
+                gap_distance="{gap_distance}"
+                frame_name="${arg0}"
+                
+                server_name="/gripper_cmd" 
+                server_timeout="10" 
+                cancel_timeout="5" />
+
+            <Action ID="MoveToPose"  
+                robot_name="${arg3}" 
+                pose="{pre_grasp_pose}"
+
+                server_name="/move_topose" 
+                server_timeout="10" 
+                cancel_timeout="5" />
+
+
         </Sequence>
     </BehaviorTree>
     <!-- ////////// -->
@@ -35,16 +64,18 @@
 
             <output_port name="grasp_pose"/>
             <output_port name="pre_grasp_pose"/>
+            <output_port name="pre_gap_distance"/>
             <output_port name="gap_distance"/>
         </Action>
         
         <Action ID="MoveToPose">
-            <input_port name="arm_group"/>
+            <input_port name="robot_name"/>
             <input_port name="pose"/>
         </Action>
         
         <Action ID="GripperCmd">
             <input_port name="gap_distance"/>
+            <input_port name="frame_name"/>
         </Action>
     
     </TreeNodesModel>

robossembler/include/robot_bt/behavior_tree_nodes/GetGraspPoses.hpp

@@ -27,6 +27,7 @@ public:
             BT::InputPort<std::string>("part_id"),
             BT::OutputPort<geometry_msgs::msg::Pose>("grasp_pose"),
             BT::OutputPort<geometry_msgs::msg::Pose>("pre_grasp_pose"),
+            BT::OutputPort<double>("pre_gap_distance"),
             BT::OutputPort<double>("gap_distance")
         });
     }

robossembler/src/behavior_tree_nodes/atomic_skills/GetGraspPoses.cpp

@@ -46,6 +46,7 @@ BT::NodeStatus GetGraspPoses::handle_response(GetGraspPosesSrv::Response::Shared
 
     setOutput<geometry_msgs::msg::Pose>("grasp_pose", grasp_pose.grasp_pose);
     setOutput<geometry_msgs::msg::Pose>("pre_grasp_pose", grasp_pose.pre_grasp_pose);
+    setOutput<double>("pre_gap_distance", grasp_pose.gap_distance+0.006);
     setOutput<double>("gap_distance", grasp_pose.gap_distance);
 
     return BT::NodeStatus::SUCCESS;

robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp

@@ -12,26 +12,19 @@ class SendGripperCmd : public BtAction<GripperCmd>
 {
     public:
         SendGripperCmd(const std::string & name, const BT::NodeConfiguration & config) 
-        : BtAction<GripperCmd>(name, config) {
+        : BtAction<GripperCmd>(name, config) 
-            // //gripper_gap = 0.02;
+        {
-            // gripper_point_a_ = 0.06;
-            // gripper_point_b_ = 0.05;
-
-            // gripper_cmd.insert(std::make_pair("open", gripper_point_a_));
-            // gripper_cmd.insert(std::make_pair("close", gripper_point_b_));
-
         }
 
         GripperCmd::Goal populate_goal() override
         {
 
             auto goal = GripperCmd::Goal();
-            // getInput<std::string>("command", command);
-            //goal.value = gripper_cmd[command];
-            //goal.trajectory.points = gripper_cmd[command];
 
             getInput<double>("gap_distance", distance_);
+            getInput<std::string>("frame_name", frame_);
             goal.value = distance_;
+            goal.frame = frame_;
 
             return goal;
         }
@@ -39,16 +32,15 @@ class SendGripperCmd : public BtAction<GripperCmd>
         static PortsList providedPorts()
         {
             return providedBasicPorts({
-                    InputPort<double>("gap_distance")});
+                InputPort<double>("gap_distance"),
+                InputPort<std::string>("frame_name")
+            });
         }
 
     private:
         std::vector<std::string> joint_names_ = {"rasmt_Slide_1", "rasmt_Slide_2"};
-        // double gripper_point_a_{0.0};
-        // double gripper_point_b_{0.0};
-        // std::map<std::string, double> gripper_cmd;
-        // std::string command;
         double distance_;
+        std::string frame_;
 
 }; // class MoveToJointState
 

robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp

@@ -15,53 +15,27 @@ class MoveToPose : public BtAction<MoveToPoseAction>
 {
     public:
         MoveToPose(const std::string & name, const BT::NodeConfiguration & config) 
-        : BtAction<MoveToPoseAction>(name, config) {
+        : BtAction<MoveToPoseAction>(name, config) 
-            // target_pose_a_.position.x = 0.519;//0.40644;
+        {
-            // target_pose_a_.position.y = 0.5;//0.0;
-            // target_pose_a_.position.z = -0.021;//0.3274;
-            // target_pose_a_.orientation.x = 0.58;//0;
-            // target_pose_a_.orientation.y = -0.16;//1.0;
-            // target_pose_a_.orientation.z = 0.16;//0.0;
-            // target_pose_a_.orientation.w = 0.77;//0.009;
-
-            // target_pose_b_.position.x = 0.0;
-            // target_pose_b_.position.y = 0.0;
-            // target_pose_b_.position.z = -0.12;
-            // target_pose_b_.orientation.x = 0;
-            // target_pose_b_.orientation.y = 0.0;
-            // target_pose_b_.orientation.z = 0.0;
-            // target_pose_b_.orientation.w = 0.0;
-
-            // target_pose_c_.position.x = 0.0;
-            // target_pose_c_.position.y = 0.0;
-            // target_pose_c_.position.z = 0.12;
-            // target_pose_c_.orientation.x = 0;
-            // target_pose_c_.orientation.y = 0.0;
-            // target_pose_c_.orientation.z = 0.0;
-            // target_pose_c_.orientation.w = 0.0;
-            
-            // pick_and_place_command.insert(std::make_pair("go_to", target_pose_a_));
-            // pick_and_place_command.insert(std::make_pair("down", target_pose_b_));
-            // pick_and_place_command.insert(std::make_pair("up", target_pose_c_));
-
-
         }
 
         MoveToPoseAction::Goal populate_goal() override
         {
-            getInput<std::string>("arm_group", robot_name_);
+            getInput<std::string>("robot_name", robot_name_);
-            //robot_name_ = getInput<std::string>("arm_group").value();
             getInput<geometry_msgs::msg::Pose>("pose", pose_);
 
-            RCLCPP_INFO(_node->get_logger(), "GrasPose \npose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f", 
+            RCLCPP_INFO(_node->get_logger(), "GrasPose pose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f", 
                 pose_.position.x, pose_.position.y, pose_.position.z,
                 pose_.orientation.x, pose_.orientation.y, pose_.orientation.z, pose_.orientation.w);
-            RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
+
             auto goal = MoveToPoseAction::Goal();
+            RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
+            
             goal.robot_name = robot_name_;
+            goal.target_pose = pose_;
             goal.end_effector_acceleration = 1.0;
             goal.end_effector_velocity = 1.0;
-            goal.target_pose = pose_;
+            
             RCLCPP_INFO(_node->get_logger(), "Goal populated");
 
             return goal;
@@ -70,17 +44,14 @@ class MoveToPose : public BtAction<MoveToPoseAction>
         static PortsList providedPorts()
         {
             return providedBasicPorts({
-                    InputPort<std::string>("arm_group"),
+                InputPort<std::string>("robot_name"),
-                    InputPort<geometry_msgs::msg::Pose>("pose")});
+                InputPort<geometry_msgs::msg::Pose>("pose")
+            });
         }
 
     private:
-        geometry_msgs::msg::Pose target_pose_a_;
-        geometry_msgs::msg::Pose target_pose_b_, target_pose_c_;
         std::string robot_name_;
-        // std::map<std::string, geometry_msgs::msg::Pose> pick_and_place_command;
         geometry_msgs::msg::Pose pose_;
-        // std::string pick;
 
 }; // class MoveToPose
 

robossembler/src/move_controller_node.cpp

@@ -55,7 +55,7 @@ public:
     void init_knowledge()
     {
         problem_expert_->addInstance(plansys2::Instance{"printer1", "printer"});
-        problem_expert_->addInstance(plansys2::Instance{"output_shaft_211118", "part"});
+        problem_expert_->addInstance(plansys2::Instance{"box1", "part"});
         problem_expert_->addInstance(plansys2::Instance{"rasmt_arm_group", "arm"});
         problem_expert_->addInstance(plansys2::Instance{"workspace1", "workspace"});
 
@@ -65,7 +65,7 @@ public:
         problem_expert_->addPredicate(plansys2::Predicate{"(printer_ready printer1)"});
         problem_expert_->addPredicate(plansys2::Predicate{"(arm_available rasmt_arm_group)"});
 
-        problem_expert_->setGoal(plansys2::Goal("(and(part_at output_shaft_211118 workspace1))"));
+        problem_expert_->setGoal(plansys2::Goal("(and(part_at box1 workspace1))"));
         // problem_expert_->setGoal(plansys2::Goal("(and(enviroment_setup rasmt_arm_group rasmt_hand_arm_group cube))"));
 
     }

robossembler_interfaces/action/GripperCommand.action

@@ -1,4 +1,5 @@
 float64 value
+string frame
 ---
 bool success
 ---

robossembler_scene_monitor/scene_monitor/config/component_state_node.param.yaml

@@ -2,11 +2,19 @@ component_state_monitor_node:
   ros__parameters:
     models-package: "sdf_models"
     global_frame: "world"
-    printers_names: ["printer1"]
+    printers_names: ["printer1", "printer2"]
     printers:
       printer1:
-        position: [0.5, 0.0, 0.12]
+        position: [0.3, 0.3, 0.012]
+        orientation: [0.0, 0.0, 0.0, 1.0]
+      printer2:
+        position: [0.0, 0.5, 0.012]
         orientation: [0.0, 0.0, 0.0, 0.0]
-      # printerB:
+    workspaces_names: ["workspace1", "workspace2"]
-      #   position: [2.0, 2.0, 2.0]
+    workspaces:
-      #   orientation: [2.0, 2.0, 2.0, 2.0]
+      workspace1:
+        position: [-0.5, 0.0, 0.012]
+        orientation: [0.0, 0.0, 0.0, 0.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

@@ -29,6 +29,12 @@ enum ComponentState
     Posted
 };
 
+enum ComponentType
+{
+    Part,
+    Assembly
+};
+
 class Component
 {
 public:
@@ -37,6 +43,8 @@ public:
     Component(const std::string &frame_name);
 
     ComponentState getCurrentState() const;
+    ComponentType getType() const;
+    std::vector<std::string> getComponentLinks() const;
     std::string getFrameName() const;
     scene_monitor_interfaces::msg::GraspPose getGraspPose(const std::string &grasp_pose_name) const;
     std::vector<scene_monitor_interfaces::msg::GraspPose> getGraspPoses() const;
@@ -52,8 +60,10 @@ private:
 
     std::string _frame_name;
     ComponentState _current_state;
+    ComponentType _component_type;
     geometry_msgs::msg::Pose _placement;
     std::map<std::string, scene_monitor_interfaces::msg::GraspPose> _grasp_poses;
+    std::vector<std::string> _component_links;
 
     geometry_msgs::msg::Pose _current_pose;
 };

robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp

@@ -14,6 +14,7 @@
 #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 "component.hpp"
 #include "component_spawner.hpp"
@@ -26,6 +27,7 @@ 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;
 
 class ComponentStateMonitor: public rclcpp::Node
 {
@@ -47,11 +49,12 @@ private:
         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 makeTransform(const Component& component, const std::string& parent);
     void makeTransform(const std::string& frame_name, const geometry_msgs::msg::Pose& frame_pose, 
                                                                       const std::string& parent);
-    void updateVizMarker(const Component& component);
     void updateComponents();
 
     std::map<std::string, Component> _loaded_components;
@@ -59,9 +62,11 @@ private:
     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;
 
     std::string _models_package;
     std::map<std::string, geometry_msgs::msg::Pose> _loaded_printers;
+    std::map<std::string, geometry_msgs::msg::Pose> _loaded_workspaces;
     ComponentSpawner _component_spawner_node;
     ComponentPoseEstimation _pose_estimation_node;
 

robossembler_scene_monitor/scene_monitor/include/scene_state_monitor.hpp

@@ -1,20 +1,13 @@
-#include "component_state_monitor.hpp"
+#pragma once 
+
+#include <rclcpp/rclcpp.hpp>
 
 namespace scene_state_monitor
 {
 
-
 class SceneStateMonitor: public rclcpp::Node
 {
 
-public:
-
-    SceneStateMonitor();
-    ~SceneStateMonitor();
-
-private:
-
-    component_state_monitor::ComponentStateMonitor _component_state_monitor_node;
-
 };
+
 }

robossembler_scene_monitor/scene_monitor/package.xml

@@ -14,6 +14,7 @@
   <depend>tf2_ros</depend>
   <depend>std_msgs</depend>
   <!-- <depend>visualization_msgs</depend> -->
+  <depend>gazebo_msgs</depend>
   <depend>geometry_msgs</depend>
   <!-- <depend>action_msgs</depend> -->
   <depend>scene_monitor_interfaces</depend>

robossembler_scene_monitor/scene_monitor/src/component.cpp

@@ -22,6 +22,16 @@ namespace component_state_monitor
         return _frame_name;
     }
 
+    ComponentType Component::getType() const
+    {
+        return _component_type;
+    }
+
+    std::vector<std::string> Component::getComponentLinks() const
+    {
+        return _component_links;
+    }
+
     scene_monitor_interfaces::msg::GraspPose Component::getGraspPose(const std::string &grasp_pose_name) const
     {
         if (_grasp_poses.find(grasp_pose_name) == _grasp_poses.end())
@@ -95,6 +105,18 @@ namespace component_state_monitor
             return; 
         }
 
+        std::string type = input.at("type");
+        _component_type = type == "part"? ComponentType::Part: ComponentType::Assembly;
+
+        size_t size = input.at("include").size();
+        if (size > 0)
+        {
+            for (auto it=input.at("include").begin(); it != input.at("include").end(); ++it)
+            {
+                _component_links.push_back(it.value());
+            }
+        }
+
         _placement = construct_pose(input.at("features").at("placements").at("placement"));
 
         auto grasp_poses = input.at("features").at("grasp-poses");
@@ -105,8 +127,7 @@ namespace component_state_monitor
             grasp_pose.grasp_pose = construct_pose(it.value().at("placement"));
             
             geometry_msgs::msg::Pose pre_pose;
-            pre_pose.position.z = 0.12;
+            pre_pose.position.z -= 0.05;
-            //pre_pose.orientation = grasp_pose.grasp_pose.orientation;
 
             grasp_pose.pre_grasp_pose = pre_pose;
 

robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp

@@ -10,11 +10,12 @@
 namespace component_state_monitor
 {
     using namespace std::chrono_literals;
-    ComponentStateMonitor::ComponentStateMonitor(): Node("component_state_monitor_node")
+    ComponentStateMonitor::ComponentStateMonitor(): Node("component_state_monitor")
     {   
         this->declare_parameter<std::string>("models-package", "sdf_models");
         this->declare_parameter<std::string>("global_frame", "world");
         this->declare_parameter<std::vector<std::string>>("printers_names", std::vector<std::string>{});
+        this->declare_parameter<std::vector<std::string>>("workspaces_names", std::vector<std::string>{});
 
         RCLCPP_INFO(this->get_logger(), "Initialize [get_part] service");
         _get_part_service = this->create_service<GetPartSrv>("scene_monitor/get_part", 
@@ -35,7 +36,11 @@ namespace component_state_monitor
         _print_part_service = this->create_service<PrintPartSrv>("scene_monitor/print_part", 
                                         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));
         
         _tf2_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
 
@@ -57,6 +62,15 @@ namespace component_state_monitor
             if (component.second.getCurrentState() == ComponentState::Printed)
             {
                 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)
                 {
@@ -146,11 +160,43 @@ namespace component_state_monitor
 
             tmp_pose.orientation.x = orientation.at(0);
             tmp_pose.orientation.y = orientation.at(1);
-            tmp_pose.orientation.y = orientation.at(2);
+            tmp_pose.orientation.z = orientation.at(2);
-            tmp_pose.orientation.z = orientation.at(3);
+            tmp_pose.orientation.w = orientation.at(3);
 
             _loaded_printers.insert({printer, tmp_pose});
         }
+
+        std::vector<std::string> workspaces;
+        this->get_parameter("workspaces_names", workspaces);
+
+        for (const auto& workspace: workspaces)
+        {
+            std::vector<double> position;
+            std::vector<double> orientation;
+
+            this->declare_parameter<std::vector<double>>("workspaces." + workspace + ".position", std::vector<double>{0.0, 0.0, 0.0});
+            this->declare_parameter<std::vector<double>>("workspaces." + workspace + ".orientation", std::vector<double>{0.0, 0.0, 0.0, 0.0});
+
+            this->get_parameter("workspaces." + workspace + ".position", position);
+            RCLCPP_INFO(this->get_logger(), "Initialize with printer (%s) position (%f, %f, %f)",
+                workspace.c_str(), position.at(0), position.at(1), position.at(2));
+
+            this->get_parameter("workspaces." + workspace + ".orientation", orientation);
+            RCLCPP_INFO(this->get_logger(), "Initialize with printer (%s) orientation (%f, %f, %f, %f)",
+                workspace.c_str(), orientation.at(0), orientation.at(1), orientation.at(2), orientation.at(3));
+            
+            geometry_msgs::msg::Pose tmp_pose;
+            tmp_pose.position.x = position.at(0);
+            tmp_pose.position.y = position.at(1);
+            tmp_pose.position.z = position.at(2);
+
+            tmp_pose.orientation.x = orientation.at(0);
+            tmp_pose.orientation.y = orientation.at(1);
+            tmp_pose.orientation.z = orientation.at(2);
+            tmp_pose.orientation.w = orientation.at(3);
+
+            _loaded_workspaces.insert({workspace, tmp_pose});
+        }
     }
 
     ComponentStateMonitor::~ComponentStateMonitor() {}
@@ -179,6 +225,46 @@ 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)
+    {
+        RCLCPP_INFO(this->get_logger(), "[get_workspace_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());
+            auto workspace = _loaded_workspaces.at(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 + ")";
+        } else {
+
+            RCLCPP_WARN(this->get_logger(), "[get_workspace_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");
+    }
+
+    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::getGraspPoses(const std::shared_ptr<GetGraspPosesSrv::Request> request,
         std::shared_ptr<GetGraspPosesSrv::Response> response)
     {
@@ -195,28 +281,12 @@ namespace component_state_monitor
                 geometry_msgs::msg::TransformStamped transformStamped = _tf2_buffer->lookupTransform(
                     _global_frame, pose.label, tf2::TimePointZero 
                 );
-
+                pose.grasp_pose = transformToPose(transformStamped);
-                pose.grasp_pose.position.x = transformStamped.transform.translation.x;
-                pose.grasp_pose.position.y = transformStamped.transform.translation.y;
-                pose.grasp_pose.position.z = transformStamped.transform.translation.z;
-
-                pose.grasp_pose.orientation.x = transformStamped.transform.rotation.x;
-                pose.grasp_pose.orientation.y = transformStamped.transform.rotation.y;
-                pose.grasp_pose.orientation.z = transformStamped.transform.rotation.z;
-                pose.grasp_pose.orientation.w = transformStamped.transform.rotation.w;
 
                 geometry_msgs::msg::TransformStamped preTransformStamped = _tf2_buffer->lookupTransform(
                     _global_frame, std::string("pre_"+pose.label), tf2::TimePointZero 
                 );
-
+                pose.pre_grasp_pose = transformToPose(preTransformStamped);
-                pose.pre_grasp_pose.position.x = preTransformStamped.transform.translation.x;
-                pose.pre_grasp_pose.position.y = preTransformStamped.transform.translation.y;
-                pose.pre_grasp_pose.position.z = preTransformStamped.transform.translation.z;
-
-                pose.pre_grasp_pose.orientation.x = preTransformStamped.transform.rotation.x;
-                pose.pre_grasp_pose.orientation.y = preTransformStamped.transform.rotation.y;
-                pose.pre_grasp_pose.orientation.z = preTransformStamped.transform.rotation.z;
-                pose.pre_grasp_pose.orientation.w = preTransformStamped.transform.rotation.w;
 
                 result_poses.push_back(pose);
             }
@@ -251,27 +321,13 @@ namespace component_state_monitor
 
                 auto grasp_pose = component.getGraspPose(request->grasp_pose_name);
 
-                grasp_pose.grasp_pose.position.x = transformStamped.transform.translation.x;
+                grasp_pose.grasp_pose = transformToPose(transformStamped);
-                grasp_pose.grasp_pose.position.y = transformStamped.transform.translation.y;
-                grasp_pose.grasp_pose.position.z = transformStamped.transform.translation.z;
-
-                grasp_pose.grasp_pose.orientation.x = transformStamped.transform.rotation.x;
-                grasp_pose.grasp_pose.orientation.y = transformStamped.transform.rotation.y;
-                grasp_pose.grasp_pose.orientation.z = transformStamped.transform.rotation.z;
-                grasp_pose.grasp_pose.orientation.w = transformStamped.transform.rotation.w;
 
                 geometry_msgs::msg::TransformStamped preTransformStamped = _tf2_buffer->lookupTransform(
                     _global_frame, std::string("pre_"+grasp_pose.label), tf2::TimePointZero 
                 );
 
-                grasp_pose.pre_grasp_pose.position.x = preTransformStamped.transform.translation.x;
+                grasp_pose.pre_grasp_pose = transformToPose(preTransformStamped);
-                grasp_pose.pre_grasp_pose.position.y = preTransformStamped.transform.translation.y;
-                grasp_pose.pre_grasp_pose.position.z = preTransformStamped.transform.translation.z;
-
-                grasp_pose.pre_grasp_pose.orientation.x = preTransformStamped.transform.rotation.x;
-                grasp_pose.pre_grasp_pose.orientation.y = preTransformStamped.transform.rotation.y;
-                grasp_pose.pre_grasp_pose.orientation.z = preTransformStamped.transform.rotation.z;
-                grasp_pose.pre_grasp_pose.orientation.w = preTransformStamped.transform.rotation.w;
 
                 response->grasp_pose = grasp_pose;
                 response->success = true;
@@ -300,6 +356,7 @@ namespace component_state_monitor
         if (_loaded_components.find(request->frame_id) != _loaded_components.end())
         {
             auto component = _loaded_components.at(request->frame_id);
+
             if (_loaded_printers.find(request->printer_id) != _loaded_printers.end())
             {
                 auto printer_pose = _loaded_printers.at(request->printer_id);

robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt

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

robossembler_scene_monitor/scene_monitor_interfaces/srv/GetWorkspacePose.srv

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

robossembler_servers/src/gripper_cmd.cpp

@@ -87,13 +87,14 @@ private:
         }
         // Compute finger change for detach or attach
         auto finger_state_change = goal->value - finger_state;
+        auto frame_name = goal->frame;
         RCLCPP_ERROR(this->get_logger(), "Finger change [%.2f]", finger_state_change);
         if(finger_state_change<0.0)
         {
-            send_attach_goal("rasmt", "cube", "rasmt_Dock_Link", "link"); // TODO: read parameters from other sources
+            send_attach_goal("rasmt", frame_name, "rasmt_Dock_Link", "link"); // TODO: read parameters from other sources
         } else if (finger_state_change>0.0)
         {
-            send_detach_goal("rasmt", "cube", "rasmt_Dock_Link", "link");
+            send_detach_goal("rasmt", frame_name, "rasmt_Dock_Link", "link");
         }
         // Send commands to gripper
         std_msgs::msg::Float64MultiArray commands;

sdf_models/models/box1/frames.json

@@ -0,0 +1,40 @@
+{
+ "label": "box1",
+ "type": "component",
+ "include": [],
+ "features": {
+  "grasp-poses": {
+   "Gripper_for_Part__Feature057": {
+    "placement": {
+     "position": {
+      "x": 0.0,
+      "y": 0.0,
+      "z": 0.129
+     },
+     "orientation": {
+      "x": 0.0,
+      "y": 1.0,
+      "z": 0.0,
+      "w": 0.0
+     }
+    },
+    "distance": 0.015
+   }
+  },
+  "placements": {
+   "placement": {
+    "position": {
+     "x": 0.0,
+     "y": 0.0,
+     "z": 0.0
+    },
+    "orientation": {
+     "x": 0.0,
+     "y": 0.0,
+     "z": 0.0,
+     "w": 0.0
+    }
+   }
+  }
+ }
+}

sdf_models/models/box1/model.config

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<model>
+  <name>box1</name>
+  <sdf version="1.0">model.sdf</sdf>
+
+  <author>
+    <name>Splinter1984</name>
+    <email>rom.andrianov1984@gmail.com</email>
+  </author>
+
+  <description>
+    A box1.
+  </description>
+</model>

sdf_models/models/box1/model.sdf

@@ -0,0 +1,29 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <model name="box1">
+    <pose>0 0 0.015 0 0 0</pose>
+    <link name="link">
+      <gravity>0</gravity>
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>0.03 0.03 0.03</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.03 0.03 0.03</size>
+          </box>
+        </geometry>
+        <material>
+            <ambient>0.5 0.75 0 1</ambient>
+            <diffuse>0.7 0.9 0 1</diffuse>
+            <specular>0.2 0.2 0.2 64</specular>
+            <emissive>0.1 0 0.1 1</emissive>
+          </material>
+      </visual>
+    </link>
+  </model>
+</sdf>

sdf_models/models/box12/frames.json

@@ -0,0 +1,40 @@
+{
+ "label": "box12",
+ "type": "assembly",
+ "include": ["box1", "box2"],
+ "features": {
+  "grasp-poses": {
+   "Gripper_for_Part__Feature059": {
+    "placement": {
+     "position": {
+      "x": 0.0,
+      "y": 0.0,
+      "z": 0.129
+     },
+     "orientation": {
+      "x": 0.0,
+      "y": 1.0,
+      "z": 0.0,
+      "w": 0.0
+     }
+    },
+    "distance": 0.015
+   }
+  },
+  "placements": {
+   "placement": {
+    "position": {
+     "x": 0.0,
+     "y": 0.0,
+     "z": 0.0
+    },
+    "orientation": {
+     "x": 0.0,
+     "y": 0.0,
+     "z": 0.0,
+     "w": 0.0
+    }
+   }
+  }
+ }
+}

sdf_models/models/box12/model.config

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<model>
+  <name>box12</name>
+  <sdf version="1.0">model.sdf</sdf>
+
+  <author>
+    <name>Splinter1984</name>
+    <email>rom.andrianov1984@gmail.com</email>
+  </author>
+
+  <description>
+    A box12.
+  </description>
+</model>

sdf_models/models/box12/model.sdf

@@ -0,0 +1,19 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <model name="box12">
+    <pose>0 0 0.015 0 0 0</pose>
+    <link name="box1">
+      <pose>0 0 0 0 0 0</pose>
+      <gravity>0</gravity>
+    </link>
+    <link name="box2">
+      <pose>0 0 0.03 0 0 0</pose>
+      <gravity>0</gravity>
+    </link>
+    <joint name="joint" type="fixed">
+      <parent>box1</parent>
+      <child>box2</child>
+      <pose>0.0 0.0 0.06 0.0 0.0 0.0</pose>
+    </joint>
+  </model>
+</sdf>

sdf_models/models/box2/frames.json

@@ -0,0 +1,40 @@
+{
+ "label": "box2",
+ "type": "component",
+ "include": [],
+ "features": {
+  "grasp-poses": {
+   "Gripper_for_Part__Feature058": {
+    "placement": {
+     "position": {
+      "x": 0.0,
+      "y": 0.0,
+      "z": 0.129
+     },
+     "orientation": {
+      "x": 0.0,
+      "y": 1.0,
+      "z": 0.0,
+      "w": 0.0
+     }
+    },
+    "distance": 0.015
+   }
+  },
+  "placements": {
+   "placement": {
+    "position": {
+     "x": 0.0,
+     "y": 0.0,
+     "z": 0.0
+    },
+    "orientation": {
+     "x": 0.0,
+     "y": 0.0,
+     "z": 0.0,
+     "w": 0.0
+    }
+   }
+  }
+ }
+}

sdf_models/models/box2/model.config

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<model>
+  <name>box2</name>
+  <sdf version="1.0">model.sdf</sdf>
+
+  <author>
+    <name>Splinter1984</name>
+    <email>rom.andrianov1984@gmail.com</email>
+  </author>
+
+  <description>
+    A box2.
+  </description>
+</model>

sdf_models/models/box2/model.sdf

@@ -0,0 +1,29 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <model name="box2">
+    <pose>0 0 0.015 0 0 0</pose>
+    <link name="link">
+      <gravity>0</gravity>
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>0.03 0.03 0.03</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.03 0.03 0.03</size>
+          </box>
+        </geometry>
+        <material>
+            <ambient>0.1 0.75 0 5</ambient>
+            <diffuse>0.9 0.7 0 1</diffuse>
+            <specular>0.4 0.4 0.4 64</specular>
+            <emissive>0.2 0 0.2 2</emissive>
+          </material>
+      </visual>
+    </link>
+  </model>
+</sdf>

sdf_models/models/output_shaft_211118/frames.json

@@ -1,51 +0,0 @@
-{
- "label": "output_shaft_211118",
- "placement": {
-  "position": {
-   "x": -0.0,
-   "y": -0.0,
-   "z": -0.0
-  },
-  "orientation": {
-   "x": 0.0,
-   "y": 0.0,
-   "z": 1.0,
-   "w": 0.0
-  }
- },
- "features": {
-  "grasp-poses": {
-   "Gripper_for_Part__Feature057": {
-    "placement": {
-     "position": {
-      "x": -4e-19,
-      "y": -0.0815,
-      "z": 2.2099999999999998e-17
-     },
-     "orientation": {
-      "x": 0.7133549208900113,
-      "y": 0.4955425091967418,
-      "z": 0.4955425091967417,
-      "w": 4.38085841046605
-     }
-    },
-    "distance": 0.025
-   }
-  },
-  "placements": {
-   "placement": {
-    "position": {
-     "x": 0.0,
-     "y": 0.0,
-     "z": 0.0
-    },
-    "orientation": {
-     "x": 0.0,
-     "y": 0.0,
-     "z": 0.0,
-     "w": 0.0
-    }
-   }
-  }
- }
-}

sdf_models/models/output_shaft_211118/model.config

@@ -1,11 +0,0 @@
-<?xml version="1.0" ?>
-<model>
-  <name>output_shaft_211118</name>
-  <version>Version</version>
-  <sdf version="1.5">model.sdf</sdf>
-  <author>
-    <name>Author</name>
-    <email>Email</email>
-  </author>
-  <description>Comment</description>
-</model>

sdf_models/models/output_shaft_211118/model.sdf

@@ -1,39 +0,0 @@
-<?xml version="1.0" ?>
-<sdf version="1.5">
-  <model name="output_shaft_211118">
-    <pose>0.000000 0.021391 0.020764 0.000000 0.000000 0.000000</pose>
-    <static>false</static>
-    <self_collide>false</self_collide>
-    <link name="output_shaft_211118">
-      <pose>0.021818 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
-      <inertial>
-        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
-        <mass>0.020176</mass>
-        <inertia>
-          <ixx>0.000004</ixx>
-          <ixy>-0.000000</ixy>
-          <ixz>-0.000000</ixz>
-          <iyy>0.000007</iyy>
-          <iyz>0.000000</iyz>
-          <izz>0.000007</izz>
-        </inertia>
-      </inertial>
-      <visual name="output_shaft_211118_visual">
-        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
-        <geometry>
-          <mesh>
-            <uri>model://output_shaft_211118/meshes/output_shaft_211118.dae</uri>
-          </mesh>
-        </geometry>
-      </visual>
-      <collision name="output_shaft_211118_collision">
-        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
-        <geometry>
-          <mesh>
-            <uri>model://output_shaft_211118/meshes/output_shaft_211118.dae</uri>
-          </mesh>
-        </geometry>
-      </collision>
-    </link>
-  </model>
-</sdf>