diff --git a/rbs_bt_executor/bt_trees/PosesTest.xml b/rbs_bt_executor/bt_trees/PosesTest.xml new file mode 100644 index 0000000..c4e655b --- /dev/null +++ b/rbs_bt_executor/bt_trees/PosesTest.xml @@ -0,0 +1,21 @@ + + + + + + + + + \ No newline at end of file diff --git a/rbs_bt_executor/bt_trees/gripper_tree.xml b/rbs_bt_executor/bt_trees/gripper_tree.xml deleted file mode 100644 index 77bb8d8..0000000 --- a/rbs_bt_executor/bt_trees/gripper_tree.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/rbs_bt_executor/bt_trees/test_tree.xml b/rbs_bt_executor/bt_trees/test_tree.xml index 0cf75f8..256fbc5 100644 --- a/rbs_bt_executor/bt_trees/test_tree.xml +++ b/rbs_bt_executor/bt_trees/test_tree.xml @@ -3,56 +3,142 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rbs_bt_executor/src/GetPickPlacePoses.cpp b/rbs_bt_executor/src/GetPickPlacePoses.cpp index d2e78ba..35bb201 100644 --- a/rbs_bt_executor/src/GetPickPlacePoses.cpp +++ b/rbs_bt_executor/src/GetPickPlacePoses.cpp @@ -1,7 +1,7 @@ #include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp" #include "behaviortree_cpp_v3/bt_factory.h" #include "behavior_tree/BtService.hpp" - +#include "geometry_msgs/msg/pose.hpp" using namespace BT; using GetPickPlacePoseClient = rbs_skill_interfaces::srv::GetPickPlacePoses; @@ -19,43 +19,80 @@ public: auto request = std::make_shared(); RCLCPP_INFO(_node->get_logger(), "Start populate_request()"); object_name_ = getInput("ObjectName").value(); + grasp_direction_str_ = getInput("GraspDirection").value(); + place_direction_str_ = getInput("PlaceDirection").value(); RCLCPP_INFO_STREAM(_node->get_logger(), "Starting process for object: " << object_name_); + grasp_direction_ = convert_direction_from_string(grasp_direction_str_); + place_direction_ = convert_direction_from_string(place_direction_str_); request->object_name = object_name_; + request->grasp_direction = grasp_direction_; + request->place_direction = place_direction_; return request; } BT::NodeStatus handle_response(GetPickPlacePoseClient::Response::SharedPtr response) override { + RCLCPP_INFO(_node->get_logger(), "\n Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f \n\n\t opientation.x: %f \n\t orientation.y: %f \n\t orientation.z: %f \n\t orientation.w: %f", + response->grasp[2].position.x, response->grasp[2].position.y, response->grasp[2].position.z, + response->grasp[2].orientation.x, response->grasp[2].orientation.y, response->grasp[2].orientation.z, response->grasp[2].orientation.w); RCLCPP_INFO(_node->get_logger(), "Start handle_response()"); - setOutput>("GraspPose", response->grasp_pose); - setOutput>("PreGraspPose", response->pre_grasp_pose); - setOutput>("PostGraspPose", response->post_grasp_pose); - setOutput>("PostGraspPose2", response->post_grasp_pose_d); - setOutput>("PlacePose", response->place_pose); - setOutput>("PrePlacePose", response->pre_place_pose); - setOutput>("PostPlacePose", response->post_place_pose); - setOutput>("PrePlaceJointState", response->pre_place_joint_state); - setOutput("NextObject", response->next_object); + setOutput("GraspPose", response->grasp[0]); + setOutput("PreGraspPose", response->grasp[1]); + setOutput("PostGraspPose", response->grasp[3]); + setOutput("PostPostGraspPose", response->grasp[2]); + setOutput("PlacePose", response->place[0]); + setOutput("PrePlacePose", response->place[1]); + setOutput("PostPlacePose", response->place[1]); return BT::NodeStatus::SUCCESS; } static PortsList providedPorts() { return providedBasicPorts({ InputPort("ObjectName"), - OutputPort("ObjectName"), - OutputPort>("GraspPose"), - OutputPort>("PreGraspPose"), - OutputPort>("PostGraspPose"), - OutputPort>("PostGraspPose2"), - OutputPort>("PlacePose"), - OutputPort>("PrePlacePose"), - OutputPort>("PostPlacePose"), - OutputPort>("PrePlaceJointState"), - OutputPort("NextObject") + InputPort("GraspDirection"), // x or y or z + InputPort("PlaceDirection"), // x or y or z + OutputPort("GraspPose"), + OutputPort("PreGraspPose"), + OutputPort("PostGraspPose"), + OutputPort("PostPostGraspPose"), //TODO: change to std::vector<> + OutputPort("PlacePose"), + OutputPort("PrePlacePose"), + OutputPort("PostPlacePose"), }); } + + geometry_msgs::msg::Vector3 convert_direction_from_string(std::string str) + { + std::map directions; + geometry_msgs::msg::Vector3 vector; + directions["x"] = 1; + directions["y"] = 2; + directions["z"] = 3; + switch(directions[str]){ + case 1: + vector.x = 1; + vector.y = 0; + vector.z = 0; + break; + case 2: + vector.x = 0; + vector.y = 1; + vector.z = 0; + break; + case 3: + vector.x = 0; + vector.y = 0; + vector.z = 1; + break; + }; + return vector; + } private: std::string object_name_{}; + std::string grasp_direction_str_{}; + std::string place_direction_str_{}; + geometry_msgs::msg::Vector3 grasp_direction_{}; + geometry_msgs::msg::Vector3 place_direction_{}; }; BT_REGISTER_NODES(factory) { diff --git a/rbs_bt_executor/src/MoveToPose.cpp b/rbs_bt_executor/src/MoveToPose.cpp index d836f94..2ab0bc4 100644 --- a/rbs_bt_executor/src/MoveToPose.cpp +++ b/rbs_bt_executor/src/MoveToPose.cpp @@ -23,17 +23,7 @@ class MoveToPose : public BtAction MoveToPoseAction::Goal populate_goal() override { getInput("robot_name", robot_name_); - getInput>("pose", pose_); - - pose_des.position.x = pose_[0]; - pose_des.position.y = pose_[1]; - pose_des.position.z = pose_[2]; - pose_des.orientation.x = pose_[3]; - pose_des.orientation.y = pose_[4]; - pose_des.orientation.z = pose_[5]; - pose_des.orientation.w = pose_[6]; - - + getInput("pose", pose_des); RCLCPP_INFO(_node->get_logger(), "\n Send Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f \n\n\t opientation.x: %f \n\t orientation.y: %f \n\t orientation.z: %f \n\t orientation.w: %f", pose_des.position.x, pose_des.position.y, pose_des.position.z, pose_des.orientation.x, pose_des.orientation.y, pose_des.orientation.z, pose_des.orientation.w); @@ -55,15 +45,15 @@ class MoveToPose : public BtAction { return providedBasicPorts({ InputPort("robot_name"), - InputPort>("pose") + InputPort("pose") }); } private: std::string robot_name_; - std::vector pose_; + //std::vector pose_; geometry_msgs::msg::Pose pose_des; - std::map Poses; + //std::map Poses; // geometry_msgs::msg::Pose array_to_pose(std::vector pose_array){ diff --git a/rbs_simulation/launch/rbs_simulation.launch.py b/rbs_simulation/launch/rbs_simulation.launch.py index fc7b35c..ecb3fb4 100644 --- a/rbs_simulation/launch/rbs_simulation.launch.py +++ b/rbs_simulation/launch/rbs_simulation.launch.py @@ -417,7 +417,7 @@ def generate_launch_description(): grasp_pose_loader = Node( package="rbs_skill_servers", - executable="pick_place_pose_loader", + executable="pick_place_pose_loader_service_server", output="screen", emulate_tty=True, parameters=[ diff --git a/rbs_skill_interfaces/CMakeLists.txt b/rbs_skill_interfaces/CMakeLists.txt index 77c6357..aeb90e8 100644 --- a/rbs_skill_interfaces/CMakeLists.txt +++ b/rbs_skill_interfaces/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(moveit_msgs REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) @@ -25,7 +26,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/AssembleState.srv" "srv/GetPickPlacePoses.srv" "srv/AddPlanningSceneObject.srv" - DEPENDENCIES std_msgs geometry_msgs + DEPENDENCIES std_msgs geometry_msgs moveit_msgs ) diff --git a/rbs_skill_interfaces/srv/GetPickPlacePoses.srv b/rbs_skill_interfaces/srv/GetPickPlacePoses.srv index 10c0f92..52d3384 100644 --- a/rbs_skill_interfaces/srv/GetPickPlacePoses.srv +++ b/rbs_skill_interfaces/srv/GetPickPlacePoses.srv @@ -1,12 +1,7 @@ -string object_name -string pose_name +string object_name # name of manipulating object +string action_name # pick, place, /Not yet(insert, ... +geometry_msgs/Vector3 grasp_direction +geometry_msgs/Vector3 place_direction --- -float64[] pre_grasp_pose -float64[] grasp_pose -float64[] post_grasp_pose -float64[] post_grasp_pose_d -float64[] pre_place_pose -float64[] place_pose -float64[] post_place_pose -float64[] pre_place_joint_state -string next_object \ No newline at end of file +geometry_msgs/Pose[] grasp +geometry_msgs/Pose[] place \ No newline at end of file diff --git a/rbs_skill_servers/CMakeLists.txt b/rbs_skill_servers/CMakeLists.txt index d05704f..9474b62 100644 --- a/rbs_skill_servers/CMakeLists.txt +++ b/rbs_skill_servers/CMakeLists.txt @@ -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 $ $) + target_include_directories(pick_place_pose_loader PRIVATE + $ + $) + 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() diff --git a/rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp b/rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp new file mode 100644 index 0000000..2e4a76c --- /dev/null +++ b/rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp @@ -0,0 +1,29 @@ +#include +#include "rclcpp_components/register_node_macro.hpp" +#include +#include +#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp" +#include "rclcpp/rclcpp.hpp" +#include "moveit_msgs/msg/grasp.hpp" +#include +#include +namespace rbs_skill_actions +{ + class GetGraspPickPoseServer : public rclcpp::Node + { + public: + explicit GetGraspPickPoseServer(rclcpp::NodeOptions options); + private: + static std::vector collect_pose(const Eigen::Affine3d pose, const geometry_msgs::msg::Vector3 direction, const Eigen::Vector3d scale_vec); + rclcpp::Service::SharedPtr srv_; + std::shared_ptr tf_listener_{nullptr}; + std::unique_ptr 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 grasp_param_pose; + Eigen::Affine3d get_Affine_from_arr(const std::vector pose); + }; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GetGraspPickPoseServer); \ No newline at end of file diff --git a/rbs_skill_servers/package.xml b/rbs_skill_servers/package.xml index cd813c0..3eb215d 100644 --- a/rbs_skill_servers/package.xml +++ b/rbs_skill_servers/package.xml @@ -19,6 +19,7 @@ geometry_msgs action_msgs rbs_skill_interfaces + tf2_eigen ament_lint_auto ament_lint_common diff --git a/rbs_skill_servers/src/get_grasp_pick_pose.cpp b/rbs_skill_servers/src/get_grasp_pick_pose.cpp deleted file mode 100644 index 9725096..0000000 --- a/rbs_skill_servers/src/get_grasp_pick_pose.cpp +++ /dev/null @@ -1,71 +0,0 @@ -#include -#include -#include -#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 scene_objects, std::string current_object_name); - -void handle_service( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr response) -{ - (void)request_header; - RCLCPP_INFO( - g_node->get_logger(),"GetPoseCallback"); - std::vector 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 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 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("get_pick_place_pose_service", handle_service); - rclcpp::spin(g_node); - rclcpp::shutdown(); - g_node = nullptr; - return 0; -} \ No newline at end of file diff --git a/rbs_skill_servers/src/pick_place_pose_loader.cpp b/rbs_skill_servers/src/pick_place_pose_loader.cpp new file mode 100644 index 0000000..8e2aafc --- /dev/null +++ b/rbs_skill_servers/src/pick_place_pose_loader.cpp @@ -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(this->get_clock()); + tf_listener_ = + std::make_shared(*tf_buffer_); + + srv_ = create_service("/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 +GetGraspPlacePoseServer::collect_pose( + const Eigen::Affine3d pose, + const geometry_msgs::msg::Vector3 direction, + const Eigen::Vector3d scale_vec) +{ + std::vector 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 pose) +{ + Eigen::Vector3d point(std::vector(pose.begin(), pose.begin()+3).data()); + Eigen::Quaterniond quat(std::vector(pose.begin()+3, pose.end()).data()); + Eigen::Affine3d aff; + aff.translation() = point; + aff.linear() = quat.toRotationMatrix(); + return aff; +} \ No newline at end of file