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

add rbs_utils

Browse source
This commit is contained in:
Ilya Uraev 2023-12-11 18:31:12 +03:00
parent f1a6f4d817
commit d84e8ead97
9 changed files with 551 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

39
rbs_skill_servers/src/pick_place_pose_loader.cpp
View file

@ -4,15 +4,14 @@
#include <Eigen/src/Geometry/Quaternion.h>
#include <Eigen/src/Geometry/Transform.h>
#include <cmath>
#include <fstream>
#include <geometry_msgs/msg/detail/pose__struct.hpp>
#include <memory>
#include <nlohmann/json_fwd.hpp>
#include <rclcpp/logging.hpp>
#include <sdf/Geometry.hh>
#include <tf2_eigen/tf2_eigen.hpp>
// #include <Eigen/src/Core/Matrix.h>
// #include <Eigen/src/Geometry/Transform.h>
// #include <rclcpp/logging.hpp>
// #include <tf2/LinearMath/Quaternion.h>
// #include <tf2/exceptions.h>
// #include <tf2/time.h>
// #include <tf2_eigen/tf2_eigen.hpp>
#include <vector>
using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer;
using GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses;
@ -28,11 +27,11 @@ GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options)
srv_ = create_service<GetGraspPlacePoseService>(
"/get_pick_place_pose_service",
std::bind(&GetGraspPickPoseServer::handle_server, this,
std::bind(&GetGraspPickPoseServer::handleServer, this,
std::placeholders::_1, std::placeholders::_2));
}
void GetGraspPlacePoseServer::handle_server(
void GetGraspPlacePoseServer::handleServer(
const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
request,
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
@ -40,6 +39,11 @@ void GetGraspPlacePoseServer::handle_server(
std::string object_name =
request->object_name + "_place"; // TODO: replace with better name
// Load place pose from TF2
auto d = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example");
d->setTfData();
// rbs_utils::processAsmFolderName("asp-example");
try {
place_pose_tf = tf_buffer_->lookupTransform("world", object_name.c_str(),
tf2::TimePointZero);
@ -80,13 +84,15 @@ void GetGraspPlacePoseServer::handle_server(
Eigen::Vector3d scale_grasp(0, 0, 0.10);
Eigen::Vector3d scale_place(0, 0, 0.15);
auto path = std::getenv("RBS_ASSEMBLY_PATH");
RCLCPP_ERROR_STREAM(this->get_logger(), path);
response->grasp =
collect_pose(grasp_pose, request->grasp_direction, scale_grasp);
collectPose(grasp_pose, request->grasp_direction, scale_grasp);
response->place =
collect_pose(place_pose, request->place_direction, scale_place);
collectPose(place_pose, request->place_direction, scale_place);
}
std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collect_pose(
std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collectPose(
const Eigen::Isometry3d &graspPose,
const geometry_msgs::msg::Vector3 &move_direction,
const Eigen::Vector3d &scale_vec) {
@ -123,3 +129,12 @@ std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collect_pose(
return poses;
}
// std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::getPlacePoseJson(const nlohmann::json& json)
// {
// std::vector<geometry_msgs::msg::Pose> place_pose;
// auto env_path = std::getenv("PATH");
// RCLCPP_INFO_STREAM(this->get_logger(), env_path);
// return place_pose;
// }