add skill launch, refactor pick-place pose loader
This commit is contained in:
parent
034e172f62
commit
d72c06efea
14 changed files with 1032 additions and 191 deletions
|
|
@ -1,110 +1,125 @@
|
|||
//#include "rbs_skill_servers"
|
||||
#include "rbs_skill_servers/pick_place_pose_loader.hpp"
|
||||
#include <Eigen/src/Core/Matrix.h>
|
||||
#include <Eigen/src/Geometry/AngleAxis.h>
|
||||
#include <Eigen/src/Geometry/Quaternion.h>
|
||||
#include <Eigen/src/Geometry/Transform.h>
|
||||
#include <cmath>
|
||||
#include <rclcpp/logging.hpp>
|
||||
#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>
|
||||
|
||||
using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer;
|
||||
using GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses;
|
||||
using namespace std::chrono_literals;
|
||||
// rclcpp::Node::SharedPtr g_node = nullptr;
|
||||
|
||||
|
||||
|
||||
GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options)
|
||||
: Node("grasp_place_pose_loader", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true))
|
||||
{
|
||||
tf_buffer_ =
|
||||
std::make_unique<tf2_ros::Buffer>(this->get_clock());
|
||||
tf_listener_ =
|
||||
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
: Node("grasp_place_pose_loader",
|
||||
options.allow_undeclared_parameters(true)
|
||||
.automatically_declare_parameters_from_overrides(true)) {
|
||||
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
|
||||
srv_ = create_service<GetGraspPlacePoseService>("/get_pick_place_pose_service",
|
||||
std::bind(&GetGraspPickPoseServer::handle_server, this, std::placeholders::_1, std::placeholders::_2));
|
||||
srv_ = create_service<GetGraspPlacePoseService>(
|
||||
"/get_pick_place_pose_service",
|
||||
std::bind(&GetGraspPickPoseServer::handle_server, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
|
||||
void
|
||||
GetGraspPlacePoseServer::handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request,
|
||||
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response)
|
||||
{
|
||||
std::string rrr = "ASSEMBLE_" + request->object_name; // TODO: replace with better name
|
||||
try {
|
||||
tf_data = tf_buffer_->lookupTransform(
|
||||
"world", rrr.c_str(),
|
||||
tf2::TimePointZero);
|
||||
} catch (const tf2::TransformException & ex) {
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(), "Could not transform %s to %s: %s",
|
||||
rrr.c_str(), "world", ex.what());
|
||||
return;
|
||||
}
|
||||
// TODO: Here need to check the parameter
|
||||
// We can use another parameter library from PickNik
|
||||
grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose").as_double_array();
|
||||
RCLCPP_INFO(this->get_logger(), "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f",
|
||||
request->grasp_direction.x,
|
||||
request->grasp_direction.y,
|
||||
request->grasp_direction.z);
|
||||
RCLCPP_INFO(this->get_logger(), "\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f",
|
||||
request->place_direction.x,
|
||||
request->place_direction.y,
|
||||
request->place_direction.z);
|
||||
Eigen::Affine3d grasp_pose = get_Affine_from_arr(grasp_param_pose);
|
||||
Eigen::Affine3d place_pose = tf2::transformToEigen(tf_data);
|
||||
//std::cout << "grasp_point = " << std::endl << grasp_pose.translation() << std::endl << grasp_pose.linear() << std::endl;
|
||||
//std::cout << "place_pose = " << std::endl << place_pose.translation() << std::endl << place_pose.linear() << std::endl;
|
||||
Eigen::Vector3d vec_grasp(0.15,0.15,0.02);
|
||||
Eigen::Vector3d vec_place(0,0,0.15);
|
||||
response->grasp = collect_pose(grasp_pose, request->grasp_direction, vec_grasp);
|
||||
response->place = collect_pose(place_pose, request->place_direction, vec_place);
|
||||
void GetGraspPlacePoseServer::handle_server(
|
||||
const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
|
||||
request,
|
||||
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
|
||||
response) {
|
||||
std::string object_name =
|
||||
request->object_name + "_place"; // TODO: replace with better name
|
||||
// Load place pose from TF2
|
||||
try {
|
||||
place_pose_tf = tf_buffer_->lookupTransform("world", object_name.c_str(),
|
||||
tf2::TimePointZero);
|
||||
} catch (const tf2::TransformException &ex) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not transform %s to %s: %s",
|
||||
object_name.c_str(), "world", ex.what());
|
||||
return;
|
||||
}
|
||||
// Load grasp pose from TF2
|
||||
try {
|
||||
grasp_pose_tf = tf_buffer_->lookupTransform(
|
||||
"world", request->object_name.c_str(), tf2::TimePointZero);
|
||||
} catch (const tf2::TransformException &ex) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not transforms %s to %s: %s",
|
||||
request->object_name.c_str(), "world", ex.what());
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(this->get_logger(),
|
||||
"Grasp pose from tf \n\tx=%f\n\ty=%f\n\tz=%f",
|
||||
grasp_pose_tf.transform.translation.x,
|
||||
grasp_pose_tf.transform.translation.y,
|
||||
grasp_pose_tf.transform.translation.z);
|
||||
// TODO: Here need to check the parameter
|
||||
// We can use another parameter library from PickNik
|
||||
// grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose")
|
||||
// .as_double_array();
|
||||
RCLCPP_DEBUG(this->get_logger(),
|
||||
"\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f",
|
||||
request->grasp_direction.x, request->grasp_direction.y,
|
||||
request->grasp_direction.z);
|
||||
RCLCPP_DEBUG(this->get_logger(),
|
||||
"\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f",
|
||||
request->place_direction.x, request->place_direction.y,
|
||||
request->place_direction.z);
|
||||
|
||||
auto grasp_pose = tf2::transformToEigen(grasp_pose_tf);
|
||||
auto place_pose = tf2::transformToEigen(place_pose_tf);
|
||||
|
||||
Eigen::Vector3d scale_grasp(0, 0, 0.10);
|
||||
Eigen::Vector3d scale_place(0, 0, 0.15);
|
||||
response->grasp =
|
||||
collect_pose(grasp_pose, request->grasp_direction, scale_grasp);
|
||||
response->place =
|
||||
collect_pose(place_pose, request->place_direction, scale_place);
|
||||
}
|
||||
|
||||
std::vector<geometry_msgs::msg::Pose>
|
||||
GetGraspPlacePoseServer::collect_pose(
|
||||
const Eigen::Affine3d pose,
|
||||
const geometry_msgs::msg::Vector3 direction,
|
||||
const Eigen::Vector3d scale_vec)
|
||||
{
|
||||
std::vector<geometry_msgs::msg::Pose> pose_v_;
|
||||
pose_v_.push_back(tf2::toMsg(pose));
|
||||
Eigen::Vector3d posedir;
|
||||
Eigen::Affine3d pose_ = pose;
|
||||
tf2::fromMsg(direction, posedir);
|
||||
Eigen::Matrix3d Ixy = Eigen::Matrix3d::Zero();//posedir * posedir.transpose();
|
||||
Ixy.diagonal() = posedir;
|
||||
Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero();
|
||||
Iz.diagonal() = Eigen::Vector3d::UnitZ();
|
||||
std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collect_pose(
|
||||
const Eigen::Isometry3d &graspPose,
|
||||
const geometry_msgs::msg::Vector3 &move_direction,
|
||||
const Eigen::Vector3d &scale_vec) {
|
||||
std::vector<geometry_msgs::msg::Pose> poses{};
|
||||
// Add GraspPose as base point
|
||||
Eigen::Isometry3d grasp_pose{graspPose};
|
||||
Eigen::Quaterniond theGraspOrientation(grasp_pose.linear());
|
||||
theGraspOrientation =
|
||||
theGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
|
||||
grasp_pose.rotate(theGraspOrientation);
|
||||
poses.push_back(tf2::toMsg(grasp_pose));
|
||||
// PreGrasp calculation
|
||||
Eigen::Vector3d preGraspTranslation =
|
||||
static_cast<Eigen::Vector3d>(graspPose.translation()) + scale_vec;
|
||||
Eigen::Quaterniond preGraspOrientation(graspPose.linear());
|
||||
preGraspOrientation =
|
||||
preGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
|
||||
Eigen::Isometry3d preGraspPose = Eigen::Isometry3d::Identity();
|
||||
preGraspPose.translate(preGraspTranslation);
|
||||
preGraspPose.rotate(preGraspOrientation);
|
||||
poses.push_back(tf2::toMsg(preGraspPose));
|
||||
|
||||
if (posedir.cwiseEqual(Eigen::Vector3d::UnitX()).all()) // IF Direction == [1,0,0]
|
||||
{
|
||||
std::cout << "\n TBD" << std::endl;
|
||||
}
|
||||
else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY()).all()) // IF Direction == [0,1,0]
|
||||
{
|
||||
// Gp -- grasp point frame
|
||||
pose_.pretranslate(-(Ixy * scale_vec)); // Gp-y
|
||||
pose_v_.push_back(tf2::toMsg(pose_));
|
||||
pose_.pretranslate(Iz * scale_vec); // Gp-y + z
|
||||
pose_v_.push_back(tf2::toMsg(pose_));
|
||||
pose_.pretranslate(Ixy * scale_vec); // Gp+z
|
||||
pose_v_.push_back(tf2::toMsg(pose_));
|
||||
}
|
||||
else if (posedir.cwiseEqual(Eigen::Vector3d::UnitZ()).all()) // IF Direction == [0,0,1]
|
||||
{
|
||||
pose_.pretranslate(Ixy * scale_vec); // Choose Pre Grasp == Post Grasp
|
||||
pose_v_.push_back(tf2::toMsg(pose_)); // So calculate only Pre Grasp
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "\n TBD" << std::endl;
|
||||
}
|
||||
return pose_v_;
|
||||
}
|
||||
// PostGrasp calculation
|
||||
Eigen::Vector3d postGraspTranslation =
|
||||
static_cast<Eigen::Vector3d>(graspPose.translation()) + scale_vec;
|
||||
Eigen::Quaterniond postGraspOrientation(graspPose.linear());
|
||||
postGraspOrientation =
|
||||
postGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
|
||||
|
||||
Eigen::Affine3d
|
||||
GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector<double> pose)
|
||||
{
|
||||
Eigen::Vector3d point(std::vector<double>(pose.begin(), pose.begin()+3).data());
|
||||
Eigen::Quaterniond quat(std::vector<double>(pose.begin()+3, pose.end()).data());
|
||||
Eigen::Affine3d aff;
|
||||
aff.translation() = point;
|
||||
aff.linear() = quat.toRotationMatrix();
|
||||
return aff;
|
||||
Eigen::Isometry3d postGraspPose = Eigen::Isometry3d::Identity();
|
||||
postGraspPose.translate(postGraspTranslation);
|
||||
postGraspPose.rotate(postGraspOrientation);
|
||||
poses.push_back(tf2::toMsg(postGraspPose));
|
||||
|
||||
return poses;
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue