robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

correct merge

Browse source
This commit is contained in:
Splinter1984 2022-03-21 23:20:15 +08:00
parent f18ccccd7e
commit e6aecabde6
36 changed files with 1040 additions and 27 deletions
Download patch file Download diff file Expand all files Collapse all files

2
.gitignore vendored Normal file
View file

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

2
rasms.repos
View file

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

33
robossembler_scene_monitor/printer/include/printer_base.hpp Normal file
View file

@ -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

15
robossembler_scene_monitor/printer/src/gz_printer_plugin.cpp Normal file
View file

@ -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

14
robossembler_scene_monitor/scene_monitor/CMakeLists.txt
View file

@ -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}
)

11
robossembler_scene_monitor/scene_monitor/config/component_state_node.param.yaml Normal file
View file

@ -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]

5
robossembler_scene_monitor/scene_monitor/include/component.hpp
View file

@ -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;
};
}

23
robossembler_scene_monitor/scene_monitor/include/component_pose_estimation.hpp Normal file
View file

@ -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

36
robossembler_scene_monitor/scene_monitor/include/component_spawner.hpp Normal file
View file

@ -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

28
robossembler_scene_monitor/scene_monitor/include/component_state_monitor.hpp
View file

@ -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;
};

27
robossembler_scene_monitor/scene_monitor/launch/scene_monitor.launch.py
View file

@ -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

7
robossembler_scene_monitor/scene_monitor/package.xml
View file

@ -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>

0
robossembler_scene_monitor/scene_monitor/utils/pose_estimation/include/pose_estimation.hpp → robossembler_scene_monitor/scene_monitor/plugins/pose_estimation/include/pose_estimation.hpp
View file

0
robossembler_scene_monitor/scene_monitor/utils/pose_estimation/src/pose_estimation.cpp → robossembler_scene_monitor/scene_monitor/plugins/pose_estimation/src/pose_estimation.cpp
View file

1
robossembler_scene_monitor/scene_monitor/src/component.cpp
View file

@ -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");

55
robossembler_scene_monitor/scene_monitor/src/component_pose_estimation.cpp Normal file
View file

@ -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

109
robossembler_scene_monitor/scene_monitor/src/component_spawner.cpp Normal file
View file

@ -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;
}
}

203
robossembler_scene_monitor/scene_monitor/src/component_state_monitor.cpp
View file

@ -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

2
robossembler_scene_monitor/scene_monitor/src/component_state_monitor_node.cpp
View file

@ -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>();

31
robossembler_scene_monitor/scene_monitor_components/include/printer/printer_component_base.hpp Normal file
View file

@ -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

36
robossembler_scene_monitor/scene_monitor_components/src/printer_component.cpp Normal file
View file

@ -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)

8
robossembler_scene_monitor/scene_monitor_interfaces/CMakeLists.txt
View file

@ -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)

2
robossembler_scene_monitor/scene_monitor_interfaces/package.xml
View file

@ -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>

6
robossembler_scene_monitor/scene_monitor_interfaces/srv/PrintPart.srv Normal file
View file

@ -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

36
sdf_models/models/output_shaft_211118/frames.json Normal file
View file

@ -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
}
}
}

51
sdf_models/models/output_shaft_211118/meshes/output_shaft_211118.dae Normal file
View file

File diff suppressed because one or more lines are too long

11
sdf_models/models/output_shaft_211118/model.config Normal file
View file

@ -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>

39
sdf_models/models/output_shaft_211118/model.sdf Normal file
View file

@ -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>

36
sdf_models/models/pin_13d007/frames.json Normal file
View file

@ -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
}
}
}

51
sdf_models/models/pin_13d007/meshes/pin_13d007.dae Normal file
View file

@ -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>

11
sdf_models/models/pin_13d007/model.config Normal file
View file

@ -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>

39
sdf_models/models/pin_13d007/model.sdf Normal file
View file

@ -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>

36
sdf_models/models/pin_13d008/frames.json Normal file
View file

@ -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
}
}
}

51
sdf_models/models/pin_13d008/meshes/pin_13d008.dae Normal file
View file

@ -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>

11
sdf_models/models/pin_13d008/model.config Normal file
View file

@ -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>

39
sdf_models/models/pin_13d008/model.sdf Normal file
View file

@ -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>