Merge branch '29-freecad-to-robossembler-communication' into 7-pddl-assembly-spec

.gitignore

@@ -0,0 +1,2 @@
+ref
+*.blend1

rasms.repos

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

rasmt_support/urdf/robot/rasmt_single_macro.xacro

@@ -41,7 +41,7 @@
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0" />
       <geometry>
-        <mesh filename="package://rasmt_support/meshes/collision/Base_Link.STL" />
+        <mesh filename="package://rasmt_support/meshes/collision/Base_Link.stl" />
       </geometry>
     </collision>
   </link>
@@ -75,7 +75,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Fork_1.STL" />
+          filename="package://rasmt_support/meshes/collision/Fork_1.stl" />
       </geometry>
     </collision>
   </link>
@@ -135,7 +135,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Link_1.STL" />
+          filename="package://rasmt_support/meshes/collision/Link_1.stl" />
       </geometry>
     </collision>
   </link>
@@ -195,7 +195,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Fork_2.STL" />
+          filename="package://rasmt_support/meshes/collision/Fork_2.stl" />
       </geometry>
     </collision>
   </link>
@@ -255,7 +255,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Link_2.STL" />
+          filename="package://rasmt_support/meshes/collision/Link_2.stl" />
       </geometry>
     </collision>
   </link>
@@ -315,7 +315,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Fork_3.STL" />
+          filename="package://rasmt_support/meshes/collision/Fork_3.stl" />
       </geometry>
     </collision>
   </link>
@@ -375,7 +375,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Dock_Link.STL" />
+          filename="package://rasmt_support/meshes/collision/Dock_Link.stl" />
       </geometry>
     </collision>
   </link>

rasmt_support/urdf/tools/rasmt_hand_macro.xacro

@@ -42,7 +42,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Grip_Body.STL" />
+          filename="package://rasmt_support/meshes/collision/Grip_Body.stl" />
       </geometry>
     </collision>
   </link>
@@ -97,7 +97,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Grip_L.STL" />
+          filename="package://rasmt_support/meshes/collision/Grip_L.stl" />
       </geometry>
     </collision>
   </link>
@@ -157,7 +157,7 @@
         rpy="0 0 0" />
       <geometry>
         <mesh
-          filename="package://rasmt_support/meshes/collision/Grip_R.STL" />
+          filename="package://rasmt_support/meshes/collision/Grip_R.stl" />
       </geometry>
     </collision>
   </link>

robossembler/CMakeLists.txt

@@ -78,7 +78,7 @@ install(TARGETS
     ${plugin_libs}
     ARCHIVE DESTINATION lib
     LIBRARY DESTINATION lib
-    RUNTIME DESTINATION bin
+    RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)

robossembler_scene_monitor/printer/include/printer_base.hpp

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <string>
+#include <rclcpp/rclcpp.hpp>
+// #include <geometry_msgs/msg/pose.hpp>
+
+namespace printer
+{
+
+template <class Derived>
+class PrinterBase: public rclcpp::Node
+{
+
+    inline const Derived& derived() const& noexcept
+    {
+        return *static_cast<const Derived*>(this);
+    }
+
+public:
+    explicit PrinterBase(const rclcpp::NodeOptions& options);
+    virtual ~PrinterBase() = default;
+    // virtual void print(const std::string& frame_name, 
+    //                    const geometry_msgs::msg::Pose& placement_pose, 
+    //                    const std::string& package_name);
+    // virtual ~Printer();
+
+protected:
+    std::string _printer_name;
+    // geometry_msgs::msg::Pose _printer_pose;
+
+};
+
+} // namespace printer

robossembler_scene_monitor/printer/src/gz_printer_plugin.cpp

@@ -0,0 +1,15 @@
+#include "printer_base.hpp"
+
+namespace printer_plugins
+{
+
+class GZPrinter : public printer_base::Printer
+{
+public:
+    void init(const std::string &printer_name) override
+    {
+        this->_printer_name = printer_name;
+    }
+};
+
+}// namespace printer_plugins

robossembler_scene_monitor/scene_monitor/CMakeLists.txt

@@ -13,12 +13,21 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(ament_index_cpp REQUIRED)
-# find_package(tf2_ros REQUIRED)
+# find_package(visualization_msgs REQUIRED)
+find_package(gazebo_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 find_package(scene_monitor_interfaces REQUIRED)
 find_package(nlohmann_json REQUIRED)
 
 set(dependencies
+    # visualization_msgs
+    std_msgs  
+    tf2
+    tf2_ros
+    gazebo_msgs
     rclcpp
     geometry_msgs
     ament_index_cpp
@@ -29,6 +38,8 @@ set(dependencies
 include_directories(include)
 
 add_library(component_state_lib SHARED
+            src/component_pose_estimation.cpp
+            src/component_spawner.cpp
             src/component.cpp
             src/component_state_monitor.cpp
 )
@@ -41,6 +52,7 @@ ament_target_dependencies(component_state_monitor_node ${dependencies})
 
 install(DIRECTORY
   launch
+  config
   DESTINATION share/${PROJECT_NAME}
 )
 

robossembler_scene_monitor/scene_monitor/config/component_state_node.param.yaml

@@ -0,0 +1,11 @@
+component_state_monitor_node:
+  ros__parameters:
+    models-package: "sdf_models"
+    printers_names: ["printerA"]
+    printers:
+      printerA:
+        position: [0.5, 0.5, 0.0]
+        orientation: [0.0, 0.0, 0.0, 0.0]
+      # printerB:
+      #   position: [2.0, 2.0, 2.0]
+      #   orientation: [2.0, 2.0, 2.0, 2.0]

robossembler_scene_monitor/scene_monitor/include/component.hpp

@@ -1,7 +1,9 @@
 #pragma once
 
 #include <string>
+#include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/pose.hpp>
+
 #include "scene_monitor_interfaces/msg/grasp_pose.hpp"
 #include "nlohmann_json/json.hpp"
 
@@ -19,6 +21,7 @@ Description:
     Posted:
         - model is posted with another
 */
+
 enum ComponentState
 {
     Initialized,
@@ -45,7 +48,6 @@ public:
 
     void updateState(const ComponentState &state);
 
-
 private:
 
     std::string _frame_name;
@@ -53,6 +55,7 @@ private:
     geometry_msgs::msg::Pose _placement;
     std::map<std::string, scene_monitor_interfaces::msg::GraspPose> _grasp_poses;
 
+    geometry_msgs::msg::Pose _current_pose;
 };
 
 }

robossembler_scene_monitor/scene_monitor/include/component_pose_estimation.hpp

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <gazebo_msgs/srv/get_entity_state.hpp>
+
+namespace component_state_monitor
+{
+
+using GetEntityStateSrv = gazebo_msgs::srv::GetEntityState;
+
+class ComponentPoseEstimation: public rclcpp::Node
+{
+
+public:
+    ComponentPoseEstimation();
+    geometry_msgs::msg::Pose framePose(const std::string& frame_id);
+
+private:
+    rclcpp::Client<GetEntityStateSrv>::SharedPtr _client;
+    geometry_msgs::msg::Pose _pose;
+};
+
+}// end namespace component_state_monitor

robossembler_scene_monitor/scene_monitor/include/component_spawner.hpp

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <rclcpp/rclcpp.hpp>
+#include <gazebo_msgs/srv/spawn_entity.hpp>
+
+#include "component.hpp"
+
+namespace component_state_monitor
+{
+
+using SpawnEntitySrv = gazebo_msgs::srv::SpawnEntity;
+
+class ComponentSpawner: public rclcpp::Node
+{
+
+public:
+    ComponentSpawner();
+    ~ComponentSpawner();
+
+    bool spawnComponent(const component_state_monitor::Component& component, 
+                        const geometry_msgs::msg::Pose& printer_pose, 
+                        const std::string& printer_name);
+
+    void handleResponse(SpawnEntitySrv::Response::SharedPtr response);
+
+private:
+    rclcpp::Client<SpawnEntitySrv>::SharedPtr _spawn_component_client;
+    bool _result;
+    
+};
+
+
+} // namespace component_state_monitor

robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp

@@ -4,18 +4,28 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/service.hpp>
 #include <rclcpp/timer.hpp>
+#include <gazebo_msgs/srv/spawn_entity.hpp>
+#include <tf2_ros/static_transform_broadcaster_node.hpp>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include "scene_monitor_interfaces/srv/get_part.hpp"
 #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 "component.hpp"
+#include "component_spawner.hpp"
+#include "component_pose_estimation.hpp"
 
 namespace component_state_monitor
 {
-using GetPartSrv    = scene_monitor_interfaces::srv::GetPart;
+
+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;
 
 class ComponentStateMonitor: public rclcpp::Node
 {
@@ -35,13 +45,29 @@ private:
         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 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;
     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;
 
     std::string _models_package;
+    std::map<std::string, geometry_msgs::msg::Pose> _loaded_printers;
+    ComponentSpawner _component_spawner_node;
+    ComponentPoseEstimation _pose_estimation_node;
+
+    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> _tf2_broadcaster;
+    std::shared_ptr<tf2_ros::TransformListener> _transform_listener;
+    std::unique_ptr<tf2_ros::Buffer> _tf2_buffer;
     rclcpp::TimerBase::SharedPtr _timer;
 
 };

robossembler_scene_monitor/scene_monitor/launch/scene_monitor.launch.py

@@ -1,17 +1,26 @@
+import os
+from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
 def generate_launch_description():
     ld = LaunchDescription()
+
+    component_state_params = os.path.join(
+        get_package_share_directory('robossembler_scene_monitor'),
+        'config', 'component_state_node.param.yaml')
+
     component_state_monitor_node = Node(
-            package="robossembler_scene_monitor",
-            executable="component_state_monitor_node",
-            name="component_state_monitor_node",
-            output="screen",
-            emulate_tty=True,
-            parameters=[
-                {"models-package": "sdf_models"}
-            ]
-        )
+        package="robossembler_scene_monitor",
+        executable="component_state_monitor_node",
+        name="component_state_monitor_node",
+        output="screen",
+        emulate_tty=True,
+        parameters=[
+            component_state_params
+        ]
+    )
+    
     ld.add_action(component_state_monitor_node)
+
     return ld

robossembler_scene_monitor/scene_monitor/package.xml

@@ -10,12 +10,15 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
-  <!-- <depend>tf2_ros</depend> -->
-  <!-- <depend>rclcpp_action</depend> -->
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>std_msgs</depend>
+  <!-- <depend>visualization_msgs</depend> -->
   <depend>geometry_msgs</depend>
   <!-- <depend>action_msgs</depend> -->
   <depend>scene_monitor_interfaces</depend>
   <depend>nlohmann_json</depend>
+  <depend>printer_plugins</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

robossembler_scene_monitor/scene_monitor/src/component.cpp

@@ -95,7 +95,6 @@ namespace component_state_monitor
             return; 
         }
 
-        _frame_name = input.at("label");
         _placement = construct_pose(input.at("placement"));
 
         auto grasp_poses = input.at("features").at("grasp-poses");

robossembler_scene_monitor/scene_monitor/src/component_pose_estimation.cpp

@@ -0,0 +1,55 @@
+#include "component_pose_estimation.hpp"
+
+namespace component_state_monitor
+{
+
+using namespace std::chrono_literals;
+
+ComponentPoseEstimation::ComponentPoseEstimation(): Node("component_pose_estimation_node")
+{
+    _client = create_client<GetEntityStateSrv>("/get_entity_state");
+    while (!_client->wait_for_service(1s))
+    {
+        if (!rclcpp::ok())
+        {
+            RCLCPP_WARN(this->get_logger(), "Interruped while waiting for the server.");
+            rclcpp::shutdown();
+            return;
+        }
+        
+        RCLCPP_WARN(this->get_logger(), "Server not available, waiting again...");
+    }
+}
+
+geometry_msgs::msg::Pose ComponentPoseEstimation::framePose(const std::string& frame_id)
+{
+    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());
+
+    auto future = _client->async_send_request(request);
+
+    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::executor::FutureReturnCode::SUCCESS)
+    {
+        RCLCPP_ERROR(this->get_logger(), "Could not build future");
+        return geometry_msgs::msg::Pose();
+    }
+
+    auto result = future.get();
+    if (!result->success)
+    {
+        RCLCPP_WARN(this->get_logger(), "Get bad response from service call");
+        return geometry_msgs::msg::Pose();
+    }
+
+    _pose = result->state.pose;
+    
+    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);
+
+    return _pose;
+}
+
+}// end namespace component_state_monitor

robossembler_scene_monitor/scene_monitor/src/component_spawner.cpp

@@ -0,0 +1,109 @@
+#include "component_spawner.hpp"
+
+#include <filesystem>
+#include <fstream>
+#include <sstream>
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+
+namespace component_state_monitor
+{
+
+using namespace std::chrono_literals;
+
+ComponentSpawner::ComponentSpawner(): Node("component_spawner_node")
+{
+    _spawn_component_client = create_client<SpawnEntitySrv>("/spawn_entity");
+
+    while (!_spawn_component_client->wait_for_service(1s))
+    {
+        if (!rclcpp::ok())
+        {
+            RCLCPP_WARN(this->get_logger(), "Interruped while waiting for the server.");
+            rclcpp::shutdown();
+            return;
+        }
+        
+        RCLCPP_WARN(this->get_logger(), "Server not available, waiting again...");
+    }
+}
+
+ComponentSpawner::~ComponentSpawner() {}
+
+bool ComponentSpawner::spawnComponent(const component_state_monitor::Component& component, 
+                                      const geometry_msgs::msg::Pose& printer_pose, 
+                                      const std::string& package_name)
+{
+    auto request = std::make_shared<SpawnEntitySrv::Request>();
+    request->name = component.getFrameName();
+    RCLCPP_INFO(this->get_logger(), "Provided service call with frame name (%s)", request->name.c_str());
+
+    std::string package_share_directory = ament_index_cpp::get_package_share_directory(package_name);
+    std::filesystem::path sdf_path = package_share_directory + "/models/" + component.getFrameName() + "/model.sdf";
+    
+    RCLCPP_INFO(this->get_logger(), "Start read frame (%s) frame_path (%s)", component.getFrameName().c_str(), sdf_path.c_str());
+
+    std::ifstream sdf_file(sdf_path);
+    std::stringstream buffer;
+    
+    if (sdf_file.is_open()) {       
+        buffer << sdf_file.rdbuf();         
+        sdf_file.close();
+    } else {
+        
+        RCLCPP_WARN(this->get_logger(), "Failed while try to open file (%s)", sdf_path.c_str());
+        _result = false;
+        return _result;
+    }  
+
+    std::string xml = buffer.str();
+
+    geometry_msgs::msg::Pose spawn_pose;
+    auto placement_pose = component.getPlacementPose();
+
+    spawn_pose.position.x = printer_pose.position.x + placement_pose.position.x;
+    spawn_pose.position.y = printer_pose.position.y + placement_pose.position.y;
+    spawn_pose.position.z = printer_pose.position.z + placement_pose.position.z;
+
+    spawn_pose.orientation.x = printer_pose.orientation.x + placement_pose.orientation.x;
+    spawn_pose.orientation.y = printer_pose.orientation.y + placement_pose.orientation.y;
+    spawn_pose.orientation.z = printer_pose.orientation.z + placement_pose.orientation.z;
+    spawn_pose.orientation.w = printer_pose.orientation.w + placement_pose.orientation.w;
+
+    request->initial_pose = spawn_pose;
+    request->xml = xml;
+    RCLCPP_INFO(this->get_logger(), "Connecting to '/spawn_entity' service...");
+
+    auto future = _spawn_component_client->async_send_request(request);
+    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::executor::FutureReturnCode::SUCCESS)
+    {
+        RCLCPP_ERROR(this->get_logger(), "Could not build future");
+        return false;
+    }
+
+    auto result = future.get();
+    if (!result->success)
+    {
+        RCLCPP_WARN(this->get_logger(), "Get bad response from service call");
+        return false;
+    }
+
+    return true;
+}
+
+void ComponentSpawner::handleResponse(SpawnEntitySrv::Response::SharedPtr response)
+{
+    RCLCPP_INFO(this->get_logger(), "Handle Response call");
+    
+    if (!response->success)
+    {
+        RCLCPP_WARN(this->get_logger(), "Get failure response from service with msg: `%s`", response->status_message.c_str());
+        _result = false;
+        return;
+    }
+
+    RCLCPP_INFO(this->get_logger(), "Get success response status with msg: `%s`", response->status_message.c_str());
+    _result= true;
+}
+
+}

robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp

@@ -13,6 +13,7 @@ namespace component_state_monitor
     ComponentStateMonitor::ComponentStateMonitor(): Node("component_state_monitor_node")
     {   
         this->declare_parameter<std::string>("models-package", "sdf_models");
+        this->declare_parameter<std::vector<std::string>>("printers_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", 
@@ -29,15 +30,124 @@ namespace component_state_monitor
                                         std::bind(&ComponentStateMonitor::getGraspPose, this, 
                                         std::placeholders::_1, std::placeholders::_2));
 
+        RCLCPP_INFO(this->get_logger(), "Initialize [print_part] service");
+        _print_part_service = this->create_service<PrintPartSrv>("scene_monitor/print_part", 
+                                        std::bind(&ComponentStateMonitor::printComponent, this, 
+                                        std::placeholders::_1, std::placeholders::_2));
+
         
+        _tf2_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
+
+        _tf2_buffer = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+        _transform_listener = std::make_shared<tf2_ros::TransformListener>(*_tf2_buffer);
+
         initializeParameters();
         initializeComponents(_models_package);
+        // _viz_marker_pub = this->create_publisher<visualization_msgs::msg::MarkerArray>("scene_monitor_viz_markers", 10);
+
+        _timer = this->create_wall_timer(
+            20ms, std::bind(&ComponentStateMonitor::updateComponents, this)
+        );
+    }
+
+    void ComponentStateMonitor::updateComponents()
+    {
+        for (const auto& component: _loaded_components)
+        {
+            if (component.second.getCurrentState() == ComponentState::Printed)
+            {
+                makeTransform(component.second, "world");
+                auto grasp_poses = component.second.getGraspPoses();
+                for (const auto& grasp_pose: grasp_poses)
+                {
+                    makeTransform(grasp_pose.label, grasp_pose.placement_pose, component.first);
+                }
+            }
+        }
+    }
+
+    void ComponentStateMonitor::makeTransform(const Component& component, const std::string& parent)
+    {
+        rclcpp::Time now = this->get_clock()->now();
+        geometry_msgs::msg::TransformStamped t;
+
+        t.header.stamp = now;
+        t.header.frame_id = parent;
+        t.child_frame_id = component.getFrameName();
+
+        auto fp = _pose_estimation_node.framePose(component.getFrameName());
+
+        t.transform.translation.x = fp.position.x;
+        t.transform.translation.y = fp.position.y;
+        t.transform.translation.z = fp.position.z;
+
+        t.transform.rotation.x = fp.orientation.x;
+        t.transform.rotation.y = fp.orientation.y;
+        t.transform.rotation.z = fp.orientation.z;
+        t.transform.rotation.w = fp.orientation.w;
+
+        _tf2_broadcaster->sendTransform(t);
+    }
+
+    void ComponentStateMonitor::makeTransform(const std::string& frame_name, const geometry_msgs::msg::Pose& frame_pose,
+                                                                                              const std::string& parent)
+    {
+        rclcpp::Time now = this->get_clock()->now();
+        geometry_msgs::msg::TransformStamped t;
+
+        t.header.stamp = now;
+        t.header.frame_id = parent;
+        t.child_frame_id = frame_name;
+
+        t.transform.translation.x = frame_pose.position.x;
+        t.transform.translation.y = frame_pose.position.y;
+        t.transform.translation.z = frame_pose.position.z;
+
+        t.transform.rotation.x = frame_pose.orientation.x;
+        t.transform.rotation.y = frame_pose.orientation.y;
+        t.transform.rotation.z = frame_pose.orientation.z;
+        t.transform.rotation.w = frame_pose.orientation.w;
+
+        _tf2_broadcaster->sendTransform(t);
     }
 
     void ComponentStateMonitor::initializeParameters()
     {
         this->get_parameter("models-package", _models_package);
         RCLCPP_INFO(this->get_logger(), "Initialize with models-package (%s)", _models_package.c_str());
+        
+        std::vector<std::string> printers;
+        this->get_parameter("printers_names", printers);
+        
+        for (const auto& printer: printers)
+        {
+            std::vector<double> position;
+            std::vector<double> orientation;
+            
+            this->declare_parameter<std::vector<double>>("printers." + printer + ".position", std::vector<double>{0.0, 0.0, 0.0});
+            this->declare_parameter<std::vector<double>>("printers." + printer + ".orientation", std::vector<double>{0.0, 0.0, 0.0, 0.0});
+
+            this->get_parameter("printers." + printer + ".position", position);
+            RCLCPP_INFO(this->get_logger(), "Initialize with printer (%s) position (%f, %f, %f)",
+                printer.c_str(), position.at(0), position.at(1), position.at(2));
+
+            this->get_parameter("printers." + printer + ".orientation", orientation);
+            RCLCPP_INFO(this->get_logger(), "Initialize with printer (%s) orientation (%f, %f, %f, %f)",
+                printer.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.y = orientation.at(2);
+            tmp_pose.orientation.z = orientation.at(3);
+
+            _loaded_printers.insert({printer, tmp_pose});
+            // makeTransform(printer, tmp_pose, "world");
+        }
     }
 
     ComponentStateMonitor::~ComponentStateMonitor() {}
@@ -70,6 +180,7 @@ namespace component_state_monitor
         std::shared_ptr<GetGraspPosesSrv::Response> response)
     {
         RCLCPP_INFO(this->get_logger(), "[get_grasp_poses] Incoming request");
+
         if (_loaded_components.find(request->frame_id) != _loaded_components.end())
         {
             auto component = _loaded_components.at(request->frame_id);
@@ -90,22 +201,100 @@ namespace component_state_monitor
         std::shared_ptr<GetGraspPoseSrv::Response> response)
     {
         RCLCPP_INFO(this->get_logger(), "[get_grasp_pose] Incoming request");
+
         if (_loaded_components.find(request->frame_id) != _loaded_components.end())
         {
             auto component = _loaded_components.at(request->frame_id);
-            response->grasp_pose = component.getGraspPose(request->grasp_pose_name);
-            response->success = true;
-            response->message = "succesfully received a response from service [get_grasp_pose]";
+            
+            if (component.getCurrentState() == ComponentState::Printed)
+            {
+                // std_msgs::msg::Header header;
+                // header.frame_id = request->frame_id;
+
+                // geometry_msgs::msg::PoseStamped target_pose;
+                // target_pose.header = header;
+                // target_pose.pose = component.getGraspPose(header.frame_id).placement_pose;
+
+                // std::string world = "world";
+                // geometry_msgs::msg::PoseStamped target_pose_req = 
+                //     _tf2_buffer->transform(target_pose, world);
+                geometry_msgs::msg::TransformStamped transformStamped = _tf2_buffer->lookupTransform(
+                    "world", request->grasp_pose_name, tf2::TimePointZero 
+                );
+
+                auto grasp_pose = component.getGraspPose(request->grasp_pose_name);
+
+                grasp_pose.placement_pose.position.x = transformStamped.transform.translation.x;
+                grasp_pose.placement_pose.position.y = transformStamped.transform.translation.y;
+                grasp_pose.placement_pose.position.z = transformStamped.transform.translation.z;
+
+                grasp_pose.placement_pose.orientation.x = transformStamped.transform.rotation.x;
+                grasp_pose.placement_pose.orientation.y = transformStamped.transform.rotation.y;
+                grasp_pose.placement_pose.orientation.z = transformStamped.transform.rotation.z;
+                grasp_pose.placement_pose.orientation.w = transformStamped.transform.rotation.w;
+
+                response->grasp_pose = grasp_pose;
+                response->success = true;
+                response->message = "succesfully received a response from service [get_grasp_pose]";
+            } else {
+                
+                RCLCPP_WARN(this->get_logger(), "[get_grasp_pose] Frame with name (%s) not printed", request->frame_id.c_str());
+                response->success = false;
+                response->message = "Frame not printed";
+            }
         } else {
 
             RCLCPP_WARN(this->get_logger(), "[get_grasp_pose] Frame with name (%s) not loaded in component_state_monitor", request->frame_id.c_str());
             response->success = false;
-            response->message = "could not found frame (%s) in component_state_monitor";
+            response->message = "could not found frame in 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)
+    {
+        RCLCPP_INFO(this->get_logger(), "[print_part] Incoming request");
+
+        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);
+
+                if (_component_spawner_node.spawnComponent(component, printer_pose, _models_package))
+                {
+                    RCLCPP_INFO(this->get_logger(), "[print_part] Succesfully spawn frame (%s) at printer (%s)",
+                    request->frame_id.c_str(), request->printer_id.c_str());
+                    response->success = true;
+                    response->message = "succesfully received a response from service [print_part]";
+
+                    _loaded_components.at(request->frame_id).updateState(ComponentState::Printed);
+
+                } else {
+                    RCLCPP_WARN(this->get_logger(), "[print_part] Failed to spawn frame (%s) at printer (%s)",
+                    request->frame_id.c_str(), request->printer_id.c_str());
+                    response->success = false;
+                    response->message = "printer not available";
+                }
+            } else {
+                RCLCPP_WARN(this->get_logger(), "[print_part] Failed to spawn frame (%s) at printer (%s)",
+                request->frame_id.c_str(), request->printer_id.c_str());
+                response->success = false;
+                response->message = "could not found printer in component_state_monitor";
+            }
+        } else {
+            RCLCPP_WARN(this->get_logger(), "[print_part] Failed to spawn frame (%s) at printer (%s)",
+            request->frame_id.c_str(), request->printer_id.c_str());
+            response->success = false;
+            response->message = "could not found frame in component_state_monitor";
+        }
+
+        RCLCPP_INFO(this->get_logger(), "[print_part] Sending back response");
+    }
+
     void ComponentStateMonitor::initializeComponents(const std::string& package_name)
     {
         RCLCPP_INFO(this->get_logger(), "Start read package (%s)", package_name.c_str());
@@ -129,10 +318,9 @@ namespace component_state_monitor
             std::string frame_directory_name = entry.path().string();
             std::string frame_name = frame_directory_name.substr(frame_directory_name.find_last_of("/")+1, frame_directory_name.size()-1);
 
-            RCLCPP_INFO(this->get_logger(), "Start read frame (%s)", frame_name.c_str());
             std::filesystem::path json_path = entry.path().string() + "/frames.json";
 
-            Component component;
+            Component component(frame_name);
 
             if (!std::filesystem::exists(json_path))
             {
@@ -150,9 +338,10 @@ namespace component_state_monitor
                 component.initializeFromJson(input);
             }
 
+            component.updateState(ComponentState::Initialized);
             _loaded_components.insert({component.getFrameName(), component});
 
             RCLCPP_INFO(this->get_logger(), "Successfully initialize component (%s)", component.getFrameName().c_str());
         }
     }
-}
+} // end namespace component_state_monitor

robossembler_scene_monitor/scene_monitor/src/component_state_monitor_node.cpp

@@ -6,7 +6,7 @@
 int main(int argc, char **argv)
 {
     rclcpp::init(argc, argv);
-    rclcpp::executors::SingleThreadedExecutor exec;
+    rclcpp::executors::MultiThreadedExecutor exec;
 
     auto component_state_monitor_node = std::make_shared<component_state_monitor::ComponentStateMonitor>();
 

robossembler_scene_monitor/scene_monitor_components/include/printer/printer_component_base.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/client.hpp>
+
+#include <gazebo_msgs/srv/spawn_entity.hpp>
+
+namespace printer
+{
+
+using SpawnEntitySrv = gazebo_msgs::srv::SpawnEntity;
+class PrinterBase: public rclcpp::Node
+{
+
+public:
+    explicit PrinterBase(const rclcpp::NodeOptions& options);
+    // virtual ~PrinterBase() = default;
+
+    void PrintComponent(const std::string& printer_id, 
+                        const std::string& component_id);
+
+    void handleResponse(SpawnEntitySrv::Response::SharedPtr& response);
+
+protected:
+
+    rclcpp::Client<SpawnEntitySrv>::SharedPtr _client;
+    rclcpp::TimerBase::SharedPtr _timer;
+
+};
+
+}// end namespace printer

robossembler_scene_monitor/scene_monitor_components/src/printer_component.cpp

@@ -0,0 +1,36 @@
+#include "printer/printer_component_base.hpp"
+
+namespace printer
+{
+
+using namespace std::chrono_literals;
+PrinterBase::PrinterBase(const rclcpp::NodeOptions& options): Node("printer_component", options)
+{
+    _client = create_client<SpawnEntitySrv>("printer_client");
+    while (!_client->wait_for_service(1s))
+    {
+        if (!rclcpp::ok())
+        {
+            RCLCPP_WARN(this->get_logger(), "Interruped while waiting for the server.");
+            rclcpp::shutdown();
+            return;
+        }
+        
+        RCLCPP_WARN(this->get_logger(), "Server not available, waiting again...");
+    }
+}
+
+void PrinterBase::PrintComponent(const std::string& printer_id,
+                                 const std::string& component_id)
+{
+
+}
+
+} // end namespace printer
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(printer::PrinterBase)

robossembler_scene_monitor/scene_monitor_interfaces/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 17)
+  set(CMAKE_CXX_STANDARD 14)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -19,12 +19,14 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 
 set(msg_files
     "msg/GraspPose.msg"
 )
 
 set(srv_files
+    "srv/PrintPart.srv"
     "srv/GetGraspPose.srv"
     "srv/GetGraspPoses.srv"
     "srv/GetPart.srv"
@@ -33,7 +35,7 @@ set(srv_files
 rosidl_generate_interfaces(${PROJECT_NAME}
     ${msg_files}
     ${srv_files}
-    DEPENDENCIES geometry_msgs
+    DEPENDENCIES geometry_msgs std_msgs
 )
 
 if(BUILD_TESTING)

robossembler_scene_monitor/scene_monitor_interfaces/package.xml

@@ -11,6 +11,8 @@
   <build_depend>rosidl_default_generators</build_depend>
   
   <exec_depend>rosidl_default_runtime</exec_depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

robossembler_scene_monitor/scene_monitor_interfaces/srv/PrintPart.srv

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

sdf_models/models/output_shaft_211118/frames.json

@@ -0,0 +1,36 @@
+{
+ "features": {
+  "grasp-poses": {
+   "Gripper_for_Part__Feature057": {
+    "placement": {
+     "position": {
+      "x": -7.2e-18,
+      "y": -0.0405,
+      "z": 3.9e-18
+     },
+     "orientation": {
+      "x": -0.1745956924081881,
+      "y": 0.1745956924081881,
+      "z": -0.969036990204714,
+      "w": 1.6022436365319073
+     }
+    },
+    "distance": 42.78237402995835
+   }
+  }
+ },
+ "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
+  }
+ }
+}

sdf_models/models/output_shaft_211118/model.config

@@ -0,0 +1,11 @@
+<?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

@@ -0,0 +1,39 @@
+<?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>

sdf_models/models/pin_13d007/frames.json

@@ -0,0 +1,36 @@
+{
+ "features": {
+  "grasp-poses": {
+   "Gripper_for_Part__Feature101": {
+    "placement": {
+     "position": {
+      "x": 0.016868083435990104,
+      "y": -0.032499999999999994,
+      "z": -0.006282337239951061
+     },
+     "orientation": {
+      "x": 0.365072162647178,
+      "y": 0.6583017226394415,
+      "z": -0.6583017226394415,
+      "w": 2.4415155878685884
+     }
+    },
+    "distance": 2.9902227596730917
+   }
+  }
+ },
+ "label": "pin_13d007",
+ "placement": {
+  "position": {
+   "x": 0.0,
+   "y": 0.0,
+   "z": 0.0
+  },
+  "orientation": {
+   "x": 0.0,
+   "y": 0.0,
+   "z": 1.0,
+   "w": 0.0
+  }
+ }
+}

sdf_models/models/pin_13d007/meshes/pin_13d007.dae

@@ -0,0 +1,51 @@
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+  <asset>
+    <created>2022-03-03T17:24:28.233191</created>
+    <modified>2022-03-03T17:24:28.233191</modified>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_geometries>
+    <geometry id="geometry0" name="pin_13d007">
+      <mesh>
+        <source id="cubeverts-array0">
+          <float_array count="156" id="cubeverts-array0-array">-8 18.64331 1.355045 5 18.30034 1.469625 5 18.64331 1.355045 -8 18.30034 1.469625 -8 18.9489 1.161714 5 18.9489 1.161714 -8 19.19935 0.9008695 5 19.19935 0.9008695 5 19.38009 0.5876692 -8 19.38009 0.5876692 -8 19.48062 0.2403158 5 19.48062 0.2403158 5 19.49511 -0.121004 -8 19.49511 -0.121004 -8 19.42271 -0.4752914 5 19.42271 -0.4752914 -8 19.26762 -0.8019566 5 19.26762 -0.8019566 -8 19.03887 -1.082015 5 19.03887 -1.082015 -8 18.74974 -1.299191 5 18.74974 -1.299191 5 18.41703 -1.440862 -8 18.41703 -1.440862 -8 18.06009 -1.498796 5 18.06009 -1.498796 -8 17.69966 -1.469625 5 17.69966 -1.469625 -8 17.35669 -1.355045 5 17.35669 -1.355045 -8 17.0511 -1.161714 5 17.0511 -1.161714 5 16.80065 -0.9008695 -8 16.80065 -0.9008695 5 16.61991 -0.5876692 -8 16.61991 -0.5876692 5 16.51938 -0.2403158 -8 16.51938 -0.2403158 -8 16.50489 0.121004 5 16.50489 0.121004 5 16.57729 0.4752914 -8 16.57729 0.4752914 5 16.73238 0.8019566 -8 16.73238 0.8019566 -8 16.96113 1.082015 5 16.96113 1.082015 -8 17.25026 1.299191 5 17.25026 1.299191 -8 17.58297 1.440862 5 17.58297 1.440862 5 17.93991 1.498796 -8 17.93991 1.498796</float_array>
+          <technique_common>
+            <accessor count="52" source="#cubeverts-array0-array" stride="3">
+              <param type="float" name="X"/>
+              <param type="float" name="Y"/>
+              <param type="float" name="Z"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="cubenormals-array0">
+          <float_array count="300" id="cubenormals-array0-array">-1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15</float_array>
+          <technique_common>
+            <accessor count="100" source="#cubenormals-array0-array" stride="3">
+              <param type="float" name="X"/>
+              <param type="float" name="Y"/>
+              <param type="float" name="Z"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="cubeverts-array0-vertices">
+          <input semantic="POSITION" source="#cubeverts-array0"/>
+        </vertices>
+        <triangles count="100" material="materialref">
+          <input offset="0" semantic="VERTEX" source="#cubeverts-array0-vertices"/>
+          <input offset="1" semantic="NORMAL" source="#cubenormals-array0"/>
+          <p>0 0 1 0 2 0 0 1 3 1 1 1 4 2 2 2 5 2 4 3 0 3 2 3 6 4 7 4 8 4 6 5 5 5 7 5 6 6 4 6 5 6 9 7 6 7 8 7 10 8 11 8 12 8 10 9 8 9 11 9 10 10 9 10 8 10 13 11 10 11 12 11 14 12 12 12 15 12 14 13 13 13 12 13 16 14 15 14 17 14 16 15 14 15 15 15 18 16 17 16 19 16 18 17 16 17 17 17 20 18 21 18 22 18 20 19 19 19 21 19 20 20 18 20 19 20 23 21 20 21 22 21 24 22 22 22 25 22 24 23 23 23 22 23 26 24 25 24 27 24 26 25 24 25 25 25 28 26 26 26 27 26 28 27 27 27 29 27 30 28 31 28 32 28 30 29 29 29 31 29 30 30 28 30 29 30 33 31 32 31 34 31 33 32 30 32 32 32 35 33 34 33 36 33 35 34 33 34 34 34 37 35 35 35 36 35 38 36 39 36 40 36 38 37 36 37 39 37 38 38 37 38 36 38 41 39 40 39 42 39 41 40 38 40 40 40 43 41 41 41 42 41 44 42 42 42 45 42 44 43 43 43 42 43 46 44 45 44 47 44 46 45 44 45 45 45 48 46 49 46 50 46 48 47 47 47 49 47 48 48 46 48 47 48 51 49 48 49 50 49 3 50 50 50 1 50 3 51 51 51 50 51 2 52 1 52 50 52 2 53 50 53 49 53 34 54 39 54 36 54 31 55 34 55 32 55 11 56 8 56 7 56 11 57 7 57 5 57 11 58 5 58 2 58 25 59 29 59 27 59 19 60 17 60 15 60 19 61 15 61 12 61 21 62 49 62 47 62 21 63 47 63 45 63 21 64 45 64 42 64 21 65 42 65 40 65 21 66 40 66 39 66 21 67 31 67 29 67 21 68 25 68 22 68 21 69 12 69 11 69 21 70 39 70 34 70 21 71 19 71 12 71 21 72 2 72 49 72 21 73 34 73 31 73 21 74 11 74 2 74 21 75 29 75 25 75 3 76 0 76 51 76 41 77 37 77 38 77 6 78 9 78 4 78 4 79 9 79 0 79 37 80 28 80 35 80 35 81 28 81 33 81 33 82 28 82 30 82 41 83 28 83 37 83 10 84 13 84 9 84 51 85 13 85 48 85 0 86 13 86 51 86 9 87 13 87 0 87 28 88 24 88 26 88 14 89 16 89 13 89 24 90 20 90 23 90 18 91 20 91 16 91 48 92 20 92 46 92 46 93 20 93 44 93 44 94 20 94 43 94 43 95 20 95 41 95 13 96 20 96 48 96 41 97 20 97 28 97 28 98 20 98 24 98 16 99 20 99 13 99</p>
+        </triangles>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="scene">
+      <node id="node0" name="node0">
+        <instance_geometry url="#geometry0"/>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#scene"/>
+  </scene>
+</COLLADA>

sdf_models/models/pin_13d007/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+  <name>pin_13d007</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/pin_13d007/model.sdf

@@ -0,0 +1,39 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <model name="pin_13d007">
+    <pose>0.008000 -0.016505 0.001499 0.000000 0.000000 0.000000</pose>
+    <static>false</static>
+    <self_collide>false</self_collide>
+    <link name="pin_13d007">
+      <pose>-0.001500 0.018000 0.000000 -1.570796 1.369209 -1.570796</pose>
+      <inertial>
+        <pose>0.000000 0.000000 0.000000 -1.570796 1.369209 -1.570796</pose>
+        <mass>0.000092</mass>
+        <inertia>
+          <ixx>0.000000</ixx>
+          <ixy>-0.000000</ixy>
+          <ixz>-0.000000</ixz>
+          <iyy>0.000000</iyy>
+          <iyz>0.000000</iyz>
+          <izz>0.000000</izz>
+        </inertia>
+      </inertial>
+      <visual name="pin_13d007_visual">
+        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
+        <geometry>
+          <mesh>
+            <uri>model://pin_13d007\meshes\pin_13d007.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="pin_13d007_collision">
+        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
+        <geometry>
+          <mesh>
+            <uri>model://pin_13d007\meshes\pin_13d007.dae</uri>
+          </mesh>
+        </geometry>
+      </collision>
+    </link>
+  </model>
+</sdf>

sdf_models/models/pin_13d008/frames.json

@@ -0,0 +1,36 @@
+{
+ "features": {
+  "grasp-poses": {
+   "Gripper_for_Part__Feature102": {
+    "placement": {
+     "position": {
+      "x": 0.013874705362934565,
+      "y": -0.032499999999999994,
+      "z": 0.011467020148747559
+     },
+     "orientation": {
+      "x": 0.365072162647178,
+      "y": 0.6583017226394415,
+      "z": -0.6583017226394415,
+      "w": 2.4415155878685884
+     }
+    },
+    "distance": 2.9902227596730926
+   }
+  }
+ },
+ "label": "pin_13d008",
+ "placement": {
+  "position": {
+   "x": 0.0,
+   "y": 0.0,
+   "z": 0.0
+  },
+  "orientation": {
+   "x": 0.0,
+   "y": 0.0,
+   "z": 1.0,
+   "w": 0.0
+  }
+ }
+}

sdf_models/models/pin_13d008/meshes/pin_13d008.dae

@@ -0,0 +1,51 @@
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+  <asset>
+    <created>2022-03-03T17:24:28.081730</created>
+    <modified>2022-03-03T17:24:28.081730</modified>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_geometries>
+    <geometry id="geometry0" name="pin_13d008">
+      <mesh>
+        <source id="cubeverts-array0">
+          <float_array count="156" id="cubeverts-array0-array">-8 9.643314 16.9435 5 9.300337 17.05808 5 9.643314 16.9435 -8 9.300337 17.05808 -8 9.948904 16.75017 5 9.948904 16.75017 -8 10.19935 16.48933 5 10.19935 16.48933 5 10.38009 16.17613 -8 10.38009 16.17613 -8 10.48062 15.82877 5 10.48062 15.82877 5 10.49511 15.46745 -8 10.49511 15.46745 -8 10.42271 15.11317 5 10.42271 15.11317 -8 10.26762 14.7865 5 10.26762 14.7865 -8 10.03887 14.50644 5 10.03887 14.50644 -8 9.749736 14.28927 5 9.749736 14.28927 5 9.417033 14.1476 -8 9.417033 14.1476 -8 9.060094 14.08966 5 9.060094 14.08966 -8 8.699663 14.11883 5 8.699663 14.11883 -8 8.356686 14.23341 5 8.356686 14.23341 -8 8.051096 14.42674 5 8.051096 14.42674 5 7.800653 14.68759 -8 7.800653 14.68759 5 7.619911 15.00079 -8 7.619911 15.00079 5 7.519376 15.34814 -8 7.519376 15.34814 -8 7.504889 15.70946 5 7.504889 15.70946 5 7.577292 16.06375 -8 7.577292 16.06375 5 7.732378 16.39041 -8 7.732378 16.39041 -8 7.961134 16.67047 5 7.961134 16.67047 -8 8.250264 16.88765 5 8.250264 16.88765 -8 8.582967 17.02932 5 8.582967 17.02932 5 8.939906 17.08725 -8 8.939906 17.08725</float_array>
+          <technique_common>
+            <accessor count="52" source="#cubeverts-array0-array" stride="3">
+              <param type="float" name="X"/>
+              <param type="float" name="Y"/>
+              <param type="float" name="Z"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="cubenormals-array0">
+          <float_array count="300" id="cubenormals-array0-array">-1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 -1.332268e-15 0.200225 0.9797499 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 1 2.531565e-16 1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15 -1 -2.531565e-16 -1.308068e-15</float_array>
+          <technique_common>
+            <accessor count="100" source="#cubenormals-array0-array" stride="3">
+              <param type="float" name="X"/>
+              <param type="float" name="Y"/>
+              <param type="float" name="Z"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="cubeverts-array0-vertices">
+          <input semantic="POSITION" source="#cubeverts-array0"/>
+        </vertices>
+        <triangles count="100" material="materialref">
+          <input offset="0" semantic="VERTEX" source="#cubeverts-array0-vertices"/>
+          <input offset="1" semantic="NORMAL" source="#cubenormals-array0"/>
+          <p>0 0 1 0 2 0 0 1 3 1 1 1 4 2 2 2 5 2 4 3 0 3 2 3 6 4 7 4 8 4 6 5 5 5 7 5 6 6 4 6 5 6 9 7 6 7 8 7 10 8 11 8 12 8 10 9 8 9 11 9 10 10 9 10 8 10 13 11 10 11 12 11 14 12 12 12 15 12 14 13 13 13 12 13 16 14 15 14 17 14 16 15 14 15 15 15 18 16 17 16 19 16 18 17 16 17 17 17 20 18 21 18 22 18 20 19 19 19 21 19 20 20 18 20 19 20 23 21 20 21 22 21 24 22 22 22 25 22 24 23 23 23 22 23 26 24 25 24 27 24 26 25 24 25 25 25 28 26 26 26 27 26 28 27 27 27 29 27 30 28 31 28 32 28 30 29 29 29 31 29 30 30 28 30 29 30 33 31 32 31 34 31 33 32 30 32 32 32 35 33 34 33 36 33 35 34 33 34 34 34 37 35 35 35 36 35 38 36 39 36 40 36 38 37 36 37 39 37 38 38 37 38 36 38 41 39 40 39 42 39 41 40 38 40 40 40 43 41 41 41 42 41 44 42 42 42 45 42 44 43 43 43 42 43 46 44 45 44 47 44 46 45 44 45 45 45 48 46 49 46 50 46 48 47 47 47 49 47 48 48 46 48 47 48 51 49 48 49 50 49 3 50 50 50 1 50 3 51 51 51 50 51 2 52 1 52 50 52 2 53 50 53 49 53 32 54 42 54 40 54 32 55 40 55 39 55 32 56 39 56 36 56 32 57 36 57 34 57 29 58 47 58 45 58 29 59 45 59 42 59 29 60 32 60 31 60 29 61 42 61 32 61 25 62 29 62 27 62 17 63 15 63 12 63 17 64 12 64 11 64 21 65 49 65 47 65 21 66 25 66 22 66 21 67 19 67 17 67 21 68 11 68 8 68 21 69 8 69 7 69 21 70 7 70 5 70 21 71 5 71 2 71 21 72 2 72 49 72 21 73 47 73 29 73 21 74 29 74 25 74 21 75 17 75 11 75 3 76 0 76 51 76 51 77 0 77 48 77 43 78 33 78 41 78 41 79 33 79 38 79 38 80 33 80 37 80 37 81 33 81 35 81 46 82 28 82 44 82 44 83 28 83 43 83 33 84 28 84 30 84 43 85 28 85 33 85 28 86 24 86 26 86 14 87 16 87 13 87 13 88 16 88 10 88 24 89 20 89 23 89 18 90 20 90 16 90 10 91 20 91 9 91 9 92 20 92 6 92 6 93 20 93 4 93 4 94 20 94 0 94 48 95 20 95 46 95 0 96 20 96 48 96 16 97 20 97 10 97 46 98 20 98 28 98 28 99 20 99 24 99</p>
+        </triangles>
+      </mesh>
+    </geometry>
+  </library_geometries>
+  <library_visual_scenes>
+    <visual_scene id="scene">
+      <node id="node0" name="node0">
+        <instance_geometry url="#geometry0"/>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#scene"/>
+  </scene>
+</COLLADA>

sdf_models/models/pin_13d008/model.config

@@ -0,0 +1,11 @@
+<?xml version="1.0" ?>
+<model>
+  <name>pin_13d008</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/pin_13d008/model.sdf

@@ -0,0 +1,39 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <model name="pin_13d008">
+    <pose>0.008000 -0.007505 -0.014090 0.000000 0.000000 0.000000</pose>
+    <static>false</static>
+    <self_collide>false</self_collide>
+    <link name="pin_13d008">
+      <pose>-0.001500 0.009000 0.015588 -1.570796 1.369209 -1.570796</pose>
+      <inertial>
+        <pose>0.000000 0.000000 0.000000 -1.570796 1.369209 -1.570796</pose>
+        <mass>0.000092</mass>
+        <inertia>
+          <ixx>0.000000</ixx>
+          <ixy>-0.000000</ixy>
+          <ixz>-0.000000</ixz>
+          <iyy>0.000000</iyy>
+          <iyz>0.000000</iyz>
+          <izz>0.000000</izz>
+        </inertia>
+      </inertial>
+      <visual name="pin_13d008_visual">
+        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
+        <geometry>
+          <mesh>
+            <uri>model://pin_13d008\meshes\pin_13d008.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+      <collision name="pin_13d008_collision">
+        <pose>0.000000 0.000000 0.000000 0.000000 0.000000 0.000000</pose>
+        <geometry>
+          <mesh>
+            <uri>model://pin_13d008\meshes\pin_13d008.dae</uri>
+          </mesh>
+        </geometry>
+      </collision>
+    </link>
+  </model>
+</sdf>