update grasp _place pose loader
This commit is contained in:
parent
8e442a9e4b
commit
84798d94f8
13 changed files with 384 additions and 183 deletions
|
|
@ -28,6 +28,8 @@ find_package(tf2_ros REQUIRED)
|
|||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(rbs_skill_interfaces REQUIRED)
|
||||
find_package(rmw REQUIRED)
|
||||
find_package(tf2_eigen REQUIRED)
|
||||
find_package(tf2_msgs REQUIRED)
|
||||
|
||||
set(deps
|
||||
rclcpp
|
||||
|
|
@ -40,6 +42,8 @@ set(deps
|
|||
tf2_ros
|
||||
rclcpp_components
|
||||
rbs_skill_interfaces
|
||||
tf2_eigen
|
||||
tf2_msgs
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
|
|
@ -48,17 +52,27 @@ if(BUILD_TESTING)
|
|||
endif()
|
||||
|
||||
add_library(gripper_action_server SHARED src/gripper_control_action_server.cpp)
|
||||
add_library(pick_place_pose_loader SHARED src/pick_place_pose_loader.cpp)
|
||||
|
||||
target_include_directories(gripper_action_server PRIVATE
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
|
||||
target_include_directories(pick_place_pose_loader PRIVATE
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
|
||||
target_compile_definitions(gripper_action_server
|
||||
PRIVATE "GRIPPER_ACTION_SERVER_CPP_BUILDING_DLL")
|
||||
PRIVATE "GRIPPER_ACTION_SERVER_CPP_BUILDING_DLL")
|
||||
|
||||
target_compile_definitions(pick_place_pose_loader
|
||||
PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
|
||||
|
||||
ament_target_dependencies(gripper_action_server ${deps})
|
||||
ament_target_dependencies(pick_place_pose_loader ${deps})
|
||||
|
||||
rclcpp_components_register_node(gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer" EXECUTABLE gripper_control_action_server)
|
||||
rclcpp_components_register_node(pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer" EXECUTABLE pick_place_pose_loader_service_server)
|
||||
|
||||
|
||||
|
||||
|
|
@ -66,8 +80,8 @@ rclcpp_components_register_node(gripper_action_server PLUGIN "rbs_skill_actions:
|
|||
add_executable(move_to_joint_states_action_server src/move_to_joint_states_action_server.cpp)
|
||||
ament_target_dependencies(move_to_joint_states_action_server ${deps})
|
||||
|
||||
add_executable(pick_place_pose_loader src/get_grasp_pick_pose.cpp)
|
||||
ament_target_dependencies(pick_place_pose_loader ${deps})
|
||||
#add_executable(pick_place_pose_loader src/get_grasp_pick_pose.cpp)
|
||||
#ament_target_dependencies(pick_place_pose_loader ${deps})
|
||||
|
||||
add_executable(move_topose_action_server src/move_topose_action_server.cpp)
|
||||
ament_target_dependencies(move_topose_action_server ${deps})
|
||||
|
|
@ -78,6 +92,12 @@ ament_target_dependencies(move_cartesian_path_action_server ${deps})
|
|||
add_executable(add_planning_scene_object_service src/add_planning_scene_objects_service.cpp)
|
||||
ament_target_dependencies(add_planning_scene_object_service ${deps})
|
||||
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS
|
||||
move_topose_action_server
|
||||
|
|
@ -91,5 +111,9 @@ install(
|
|||
RUNTIME DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_export_include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
|
||||
ament_package()
|
||||
|
|
|
|||
|
|
@ -0,0 +1,29 @@
|
|||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
#include <memory>
|
||||
#include <algorithm>
|
||||
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "moveit_msgs/msg/grasp.hpp"
|
||||
#include <tf2_eigen/tf2_eigen.hpp>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
namespace rbs_skill_actions
|
||||
{
|
||||
class GetGraspPickPoseServer : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
explicit GetGraspPickPoseServer(rclcpp::NodeOptions options);
|
||||
private:
|
||||
static std::vector<geometry_msgs::msg::Pose> collect_pose(const Eigen::Affine3d pose, const geometry_msgs::msg::Vector3 direction, const Eigen::Vector3d scale_vec);
|
||||
rclcpp::Service<rbs_skill_interfaces::srv::GetPickPlacePoses>::SharedPtr srv_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
|
||||
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
geometry_msgs::msg::TransformStamped tf_data;
|
||||
void handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request,
|
||||
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response);
|
||||
std::vector<double> grasp_param_pose;
|
||||
Eigen::Affine3d get_Affine_from_arr(const std::vector<double> pose);
|
||||
};
|
||||
}
|
||||
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GetGraspPickPoseServer);
|
||||
|
|
@ -19,6 +19,7 @@
|
|||
<depend>geometry_msgs</depend>
|
||||
<depend>action_msgs</depend>
|
||||
<depend>rbs_skill_interfaces</depend>
|
||||
<depend>tf2_eigen</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
|
|
|||
|
|
@ -1,71 +0,0 @@
|
|||
#include <cinttypes>
|
||||
#include <memory>
|
||||
#include <algorithm>
|
||||
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
using GetGraspPlacePose = rbs_skill_interfaces::srv::GetPickPlacePoses;
|
||||
rclcpp::Node::SharedPtr g_node = nullptr;
|
||||
std::string pick_next_object(std::vector<std::string> scene_objects, std::string current_object_name);
|
||||
|
||||
void handle_service(
|
||||
const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<GetGraspPlacePose::Request> request,
|
||||
const std::shared_ptr<GetGraspPlacePose::Response> response)
|
||||
{
|
||||
(void)request_header;
|
||||
RCLCPP_INFO(
|
||||
g_node->get_logger(),"GetPoseCallback");
|
||||
std::vector<std::string> param_names = {
|
||||
request->object_name + ".GraspPose",
|
||||
request->object_name + ".PreGraspPose",
|
||||
request->object_name + ".PostGraspPose",
|
||||
request->object_name + ".PlacePose",
|
||||
request->object_name + ".PrePlacePose",
|
||||
request->object_name + ".PostPlacePose",
|
||||
request->object_name + ".PostGraspPose2",
|
||||
"pre_place_joint_states",
|
||||
"scene_objects"
|
||||
};
|
||||
std::vector<rclcpp::Parameter> params = g_node->get_parameters(param_names);
|
||||
for (auto ¶m : params)
|
||||
{
|
||||
RCLCPP_INFO(g_node->get_logger(), "param name: %s, value: %s",
|
||||
param.get_name().c_str(), param.value_to_string().c_str());
|
||||
}
|
||||
response->grasp_pose = params[0].as_double_array();
|
||||
response->pre_grasp_pose = params[1].as_double_array();
|
||||
response->post_grasp_pose = params[2].as_double_array();
|
||||
response->place_pose = params[3].as_double_array();
|
||||
response->pre_place_pose = params[4].as_double_array();
|
||||
response->post_place_pose = params[5].as_double_array();
|
||||
response->post_grasp_pose_d = params[6].as_double_array();
|
||||
response->pre_place_joint_state = params[7].as_double_array();
|
||||
response->next_object = pick_next_object(params[8].as_string_array(), request->object_name);
|
||||
}
|
||||
|
||||
std::string pick_next_object(std::vector<std::string> scene_objects, std::string current_object_name)
|
||||
{
|
||||
std::string next_object_name{};
|
||||
auto current_object_idx = std::find(scene_objects.begin(), scene_objects.end(), current_object_name);
|
||||
if (!(current_object_idx == scene_objects.end()))
|
||||
{
|
||||
next_object_name = *std::next(current_object_idx, 1);
|
||||
} else {
|
||||
next_object_name = "It is last object";
|
||||
}
|
||||
return next_object_name;
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions node_options;
|
||||
node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true);
|
||||
g_node = rclcpp::Node::make_shared("get_grasp_pick_pose", "", node_options);
|
||||
auto server = g_node->create_service<GetGraspPlacePose>("get_pick_place_pose_service", handle_service);
|
||||
rclcpp::spin(g_node);
|
||||
rclcpp::shutdown();
|
||||
g_node = nullptr;
|
||||
return 0;
|
||||
}
|
||||
107
rbs_skill_servers/src/pick_place_pose_loader.cpp
Normal file
107
rbs_skill_servers/src/pick_place_pose_loader.cpp
Normal file
|
|
@ -0,0 +1,107 @@
|
|||
//#include "rbs_skill_servers"
|
||||
#include "rbs_skill_servers/pick_place_pose_loader.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_);
|
||||
|
||||
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 = request->object_name + "_place"; // 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);
|
||||
}
|
||||
|
||||
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);
|
||||
std::cout << "\nPoseDir: =" << posedir << std::endl;
|
||||
Eigen::Matrix3d matIx = posedir * posedir.transpose();
|
||||
Eigen::Matrix3d matIZ = Eigen::Vector3d::UnitZ() * Eigen::Vector3d::UnitZ().transpose();
|
||||
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]
|
||||
{
|
||||
pose_.pretranslate(-(matIx * scale_vec));
|
||||
pose_v_.push_back(tf2::toMsg(pose_));
|
||||
pose_.pretranslate(matIZ * scale_vec);
|
||||
pose_v_.push_back(tf2::toMsg(pose_));
|
||||
pose_.pretranslate(matIx * scale_vec);
|
||||
pose_v_.push_back(tf2::toMsg(pose_));
|
||||
}
|
||||
else if (posedir.cwiseEqual(Eigen::Vector3d::UnitZ()).all()) // IF Direction == [0,0,1]
|
||||
{
|
||||
std::cout << "\npreplace = \n" << pose_.pretranslate(matIx * scale_vec).translation() << std::endl;
|
||||
pose_v_.push_back(tf2::toMsg(pose_));
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "\n TBD" << std::endl;
|
||||
}
|
||||
return pose_v_;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue