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

update grasp _place pose loader

Browse source
This commit is contained in:
Ilya Uraev 2023-04-21 23:28:57 +03:00
parent 8e442a9e4b
commit 84798d94f8
13 changed files with 384 additions and 183 deletions
Download patch file Download diff file Expand all files Collapse all files

77
rbs_bt_executor/src/GetPickPlacePoses.cpp
View file

@ -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<GetPickPlacePoseClient::Request>();
RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
object_name_ = getInput<std::string>("ObjectName").value();
grasp_direction_str_ = getInput<std::string>("GraspDirection").value();
place_direction_str_ = getInput<std::string>("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<std::vector<double>>("GraspPose", response->grasp_pose);
setOutput<std::vector<double>>("PreGraspPose", response->pre_grasp_pose);
setOutput<std::vector<double>>("PostGraspPose", response->post_grasp_pose);
setOutput<std::vector<double>>("PostGraspPose2", response->post_grasp_pose_d);
setOutput<std::vector<double>>("PlacePose", response->place_pose);
setOutput<std::vector<double>>("PrePlacePose", response->pre_place_pose);
setOutput<std::vector<double>>("PostPlacePose", response->post_place_pose);
setOutput<std::vector<double>>("PrePlaceJointState", response->pre_place_joint_state);
setOutput<std::string>("NextObject", response->next_object);
setOutput<geometry_msgs::msg::Pose>("GraspPose", response->grasp[0]);
setOutput<geometry_msgs::msg::Pose>("PreGraspPose", response->grasp[1]);
setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[3]);
setOutput<geometry_msgs::msg::Pose>("PostPostGraspPose", response->grasp[2]);
setOutput<geometry_msgs::msg::Pose>("PlacePose", response->place[0]);
setOutput<geometry_msgs::msg::Pose>("PrePlacePose", response->place[1]);
setOutput<geometry_msgs::msg::Pose>("PostPlacePose", response->place[1]);
return BT::NodeStatus::SUCCESS;
}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("ObjectName"),
OutputPort<std::string>("ObjectName"),
OutputPort<std::vector<double>>("GraspPose"),
OutputPort<std::vector<double>>("PreGraspPose"),
OutputPort<std::vector<double>>("PostGraspPose"),
OutputPort<std::vector<double>>("PostGraspPose2"),
OutputPort<std::vector<double>>("PlacePose"),
OutputPort<std::vector<double>>("PrePlacePose"),
OutputPort<std::vector<double>>("PostPlacePose"),
OutputPort<std::vector<double>>("PrePlaceJointState"),
OutputPort<std::string>("NextObject")
InputPort<std::string>("GraspDirection"), // x or y or z
InputPort<std::string>("PlaceDirection"), // x or y or z
OutputPort<geometry_msgs::msg::Pose>("GraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PreGraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PostGraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PostPostGraspPose"), //TODO: change to std::vector<>
OutputPort<geometry_msgs::msg::Pose>("PlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PrePlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PostPlacePose"),
});
}
geometry_msgs::msg::Vector3 convert_direction_from_string(std::string str)
{
std::map<std::string, int> 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) {

18
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -23,17 +23,7 @@ class MoveToPose : public BtAction<MoveToPoseAction>
MoveToPoseAction::Goal populate_goal() override
{
getInput<std::string>("robot_name", robot_name_);
getInput<std::vector<double>>("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<geometry_msgs::msg::Pose>("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<MoveToPoseAction>
{
return providedBasicPorts({
InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("pose")
InputPort<geometry_msgs::msg::Pose>("pose")
});
}
private:
std::string robot_name_;
std::vector<double> pose_;
//std::vector<double> pose_;
geometry_msgs::msg::Pose pose_des;
std::map<std::string, geometry_msgs::msg::Pose> Poses;
//std::map<std::string, geometry_msgs::msg::Pose> Poses;
// geometry_msgs::msg::Pose array_to_pose(std::vector<double> pose_array){