robossembler/runtime
8
0
Fork 0
Code Issues 3 Pull requests Projects Releases Packages Wiki Activity Actions

add scene_monitor services to robossebler node

Browse source
This commit is contained in:
Splinter1984 2022-03-29 02:23:01 +08:00
parent 4ed85d9811
commit b5fad5156a
47 changed files with 575 additions and 1023 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1,33 +0,0 @@
#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
View file

@ -1,15 +0,0 @@
#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

7
robossembler_scene_monitor/scene_monitor/config/component_state_node.param.yaml
View file

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

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

@ -70,5 +70,7 @@ private:
std::unique_ptr<tf2_ros::Buffer> _tf2_buffer;
rclcpp::TimerBase::SharedPtr _timer;
std::string _global_frame;
};
}

1
robossembler_scene_monitor/scene_monitor/package.xml
View file

@ -18,7 +18,6 @@
<!-- <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>

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

@ -95,15 +95,22 @@ namespace component_state_monitor
return;
}
_placement = construct_pose(input.at("placement"));
_placement = construct_pose(input.at("features").at("placements").at("placement"));
auto grasp_poses = input.at("features").at("grasp-poses");
for (auto it = grasp_poses.begin(); it != grasp_poses.end(); ++it)
{
scene_monitor_interfaces::msg::GraspPose grasp_pose;
grasp_pose.label = it.key();
grasp_pose.placement_pose = construct_pose(it.value().at("placement"));
grasp_pose.distance = it.value().at("distance");
grasp_pose.grasp_pose = construct_pose(it.value().at("placement"));
geometry_msgs::msg::Pose pre_pose;
pre_pose.position.z = 0.12;
//pre_pose.orientation = grasp_pose.grasp_pose.orientation;
grasp_pose.pre_grasp_pose = pre_pose;
grasp_pose.gap_distance = it.value().at("distance");
_grasp_poses.insert({grasp_pose.label, grasp_pose});
}

6
robossembler_scene_monitor/scene_monitor/src/component_pose_estimation.cpp
View file

@ -45,9 +45,9 @@ geometry_msgs::msg::Pose ComponentPoseEstimation::framePose(const std::string& f
_pose = result->state.pose;
RCLCPP_INFO(this->get_logger(), "Get success response status");
RCLCPP_INFO(this->get_logger(), "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);
// 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;
}

86
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::string>("global_frame", "world");
this->declare_parameter<std::vector<std::string>>("printers_names", std::vector<std::string>{});
RCLCPP_INFO(this->get_logger(), "Initialize [get_part] service");
@ -43,7 +44,6 @@ namespace component_state_monitor
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)
@ -56,11 +56,12 @@ namespace component_state_monitor
{
if (component.second.getCurrentState() == ComponentState::Printed)
{
makeTransform(component.second, "world");
makeTransform(component.second, _global_frame);
auto grasp_poses = component.second.getGraspPoses();
for (const auto& grasp_pose: grasp_poses)
{
makeTransform(grasp_pose.label, grasp_pose.placement_pose, component.first);
makeTransform(grasp_pose.label, grasp_pose.grasp_pose, component.first);
makeTransform(std::string("pre_" + grasp_pose.label), grasp_pose.pre_grasp_pose, grasp_pose.label);
}
}
}
@ -115,6 +116,9 @@ namespace component_state_monitor
{
this->get_parameter("models-package", _models_package);
RCLCPP_INFO(this->get_logger(), "Initialize with models-package (%s)", _models_package.c_str());
this->get_parameter("global_frame", _global_frame);
RCLCPP_INFO(this->get_logger(), "Initialize with global_frame (%s)", _global_frame.c_str());
std::vector<std::string> printers;
this->get_parameter("printers_names", printers);
@ -146,7 +150,6 @@ namespace component_state_monitor
tmp_pose.orientation.z = orientation.at(3);
_loaded_printers.insert({printer, tmp_pose});
// makeTransform(printer, tmp_pose, "world");
}
}
@ -184,7 +187,41 @@ namespace component_state_monitor
if (_loaded_components.find(request->frame_id) != _loaded_components.end())
{
auto component = _loaded_components.at(request->frame_id);
response->grasp_poses = component.getGraspPoses();
auto grasp_poses = component.getGraspPoses();
std::vector<scene_monitor_interfaces::msg::GraspPose> result_poses;
for (auto pose: grasp_poses)
{
geometry_msgs::msg::TransformStamped transformStamped = _tf2_buffer->lookupTransform(
_global_frame, pose.label, tf2::TimePointZero
);
pose.grasp_pose.position.x = transformStamped.transform.translation.x;
pose.grasp_pose.position.y = transformStamped.transform.translation.y;
pose.grasp_pose.position.z = transformStamped.transform.translation.z;
pose.grasp_pose.orientation.x = transformStamped.transform.rotation.x;
pose.grasp_pose.orientation.y = transformStamped.transform.rotation.y;
pose.grasp_pose.orientation.z = transformStamped.transform.rotation.z;
pose.grasp_pose.orientation.w = transformStamped.transform.rotation.w;
geometry_msgs::msg::TransformStamped preTransformStamped = _tf2_buffer->lookupTransform(
_global_frame, std::string("pre_"+pose.label), tf2::TimePointZero
);
pose.pre_grasp_pose.position.x = preTransformStamped.transform.translation.x;
pose.pre_grasp_pose.position.y = preTransformStamped.transform.translation.y;
pose.pre_grasp_pose.position.z = preTransformStamped.transform.translation.z;
pose.pre_grasp_pose.orientation.x = preTransformStamped.transform.rotation.x;
pose.pre_grasp_pose.orientation.y = preTransformStamped.transform.rotation.y;
pose.pre_grasp_pose.orientation.z = preTransformStamped.transform.rotation.z;
pose.pre_grasp_pose.orientation.w = preTransformStamped.transform.rotation.w;
result_poses.push_back(pose);
}
response->grasp_poses = result_poses;
response->success = true;
response->message = "succesfully received a response from service [get_grasp_poses]";
} else {
@ -208,30 +245,33 @@ namespace component_state_monitor
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
_global_frame, 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.grasp_pose.position.x = transformStamped.transform.translation.x;
grasp_pose.grasp_pose.position.y = transformStamped.transform.translation.y;
grasp_pose.grasp_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;
grasp_pose.grasp_pose.orientation.x = transformStamped.transform.rotation.x;
grasp_pose.grasp_pose.orientation.y = transformStamped.transform.rotation.y;
grasp_pose.grasp_pose.orientation.z = transformStamped.transform.rotation.z;
grasp_pose.grasp_pose.orientation.w = transformStamped.transform.rotation.w;
geometry_msgs::msg::TransformStamped preTransformStamped = _tf2_buffer->lookupTransform(
_global_frame, std::string("pre_"+grasp_pose.label), tf2::TimePointZero
);
grasp_pose.pre_grasp_pose.position.x = preTransformStamped.transform.translation.x;
grasp_pose.pre_grasp_pose.position.y = preTransformStamped.transform.translation.y;
grasp_pose.pre_grasp_pose.position.z = preTransformStamped.transform.translation.z;
grasp_pose.pre_grasp_pose.orientation.x = preTransformStamped.transform.rotation.x;
grasp_pose.pre_grasp_pose.orientation.y = preTransformStamped.transform.rotation.y;
grasp_pose.pre_grasp_pose.orientation.z = preTransformStamped.transform.rotation.z;
grasp_pose.pre_grasp_pose.orientation.w = preTransformStamped.transform.rotation.w;
response->grasp_pose = grasp_pose;
response->success = true;

5
robossembler_scene_monitor/scene_monitor_interfaces/msg/GraspPose.msg
View file

@ -1,3 +1,4 @@
string label
geometry_msgs/Pose placement_pose # TODO: tf2
float64 distance
geometry_msgs/Pose grasp_pose
geometry_msgs/Pose pre_grasp_pose
float64 gap_distance