update grasp _place pose loader
This commit is contained in:
parent
8e442a9e4b
commit
84798d94f8
13 changed files with 384 additions and 183 deletions
21
rbs_bt_executor/bt_trees/PosesTest.xml
Normal file
21
rbs_bt_executor/bt_trees/PosesTest.xml
Normal file
|
|
@ -0,0 +1,21 @@
|
|||
<?xml version="1.0"?>
|
||||
<root main_tree_to_execute="GripperTree">
|
||||
<BehaviorTree ID="GripperTree">
|
||||
<Sequence>
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="box1"
|
||||
GraspDirection = "y"
|
||||
PlaceDirection = "z"
|
||||
GraspPose="{GraspPose}"
|
||||
PostGraspPose="{PostGraspPose}"
|
||||
PostPostGraspPose="{PostPostGraspPose}"
|
||||
PreGraspPose="{PreGraspPose}"
|
||||
PlacePose="{PlacePose}"
|
||||
PrePlacePose="{PrePlacePose}"
|
||||
PostPlacePose="{PostPlacePose}"
|
||||
server_name="/get_pick_place_pose_service"
|
||||
server_timeout="1000"/>
|
||||
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
|
|
@ -1,19 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<root main_tree_to_execute="GripperTree">
|
||||
<BehaviorTree ID="GripperTree">
|
||||
<Sequence>
|
||||
<Action ID = "GripperControl"
|
||||
pose = "open"
|
||||
server_name="/gripper_control"
|
||||
server_timeout="10"
|
||||
cancel_timeout="5" />
|
||||
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<TreeNodesModel>
|
||||
<Action ID = "GripperControl">
|
||||
<input_port name = "pose"/>
|
||||
</Action>
|
||||
</TreeNodesModel>
|
||||
</root>
|
||||
|
|
@ -4,55 +4,141 @@
|
|||
<Sequence>
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="box1"
|
||||
GraspDirection = "y"
|
||||
PlaceDirection = "z"
|
||||
GraspPose="{GraspPose}"
|
||||
PostGraspPose="{PostGraspPose}"
|
||||
PostGraspPose2="{PostGraspPose2}"
|
||||
PostPostGraspPose="{PostPostGraspPose}"
|
||||
PreGraspPose="{PreGraspPose}"
|
||||
PlacePose="{PlacePose}"
|
||||
PrePlacePose="{PrePlacePose}"
|
||||
PostPlacePose="{PostPlacePose}"
|
||||
PrePlaceJointState="{PrePlaceJointState}"
|
||||
NextObject = "{NextObject}"
|
||||
server_name="/get_pick_place_pose_service"
|
||||
server_timeout="1000"/>
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose2}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<!-- <Action ID="MoveToJointState" robot_name="ur_manipulator_gripper" PrePlaceJointState="{PrePlaceJointState}" server_name="/move_to_joint_states" server_timeout="10000" cancel_timeout="5000" /> -->
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<!-- <Action ID="AddPlanningSceneObject" ObjectName="{NextObject}" pose="{PlacePose}" server_name="/add_planing_scene_object" server_timeout="1000"/> -->
|
||||
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Repeat num_cycles="5">
|
||||
<Sequence>
|
||||
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="{NextObject}"
|
||||
ObjectName="box2"
|
||||
GraspDirection = "y"
|
||||
PlaceDirection = "z"
|
||||
GraspPose="{GraspPose}"
|
||||
PostGraspPose="{PostGraspPose}"
|
||||
PostGraspPose2="{PostGraspPose2}"
|
||||
PostPostGraspPose="{PostPostGraspPose}"
|
||||
PreGraspPose="{PreGraspPose}"
|
||||
PlacePose="{PlacePose}"
|
||||
PrePlacePose="{PrePlacePose}"
|
||||
PostPlacePose="{PostPlacePose}"
|
||||
PrePlaceJointState="{PrePlaceJointState}"
|
||||
NextObject = "{NextObject}"
|
||||
server_name="/get_pick_place_pose_service"
|
||||
server_timeout="1000"/>
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose2}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<!-- <Action ID="MoveToJointState" robot_name="ur_manipulator_gripper" PrePlaceJointState="{PrePlaceJointState}" server_name="/move_to_joint_states" server_timeout="10000" cancel_timeout="5000" /> -->
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="box3"
|
||||
GraspDirection = "y"
|
||||
PlaceDirection = "z"
|
||||
GraspPose="{GraspPose}"
|
||||
PostGraspPose="{PostGraspPose}"
|
||||
PostPostGraspPose="{PostPostGraspPose}"
|
||||
PreGraspPose="{PreGraspPose}"
|
||||
PlacePose="{PlacePose}"
|
||||
PrePlacePose="{PrePlacePose}"
|
||||
PostPlacePose="{PostPlacePose}"
|
||||
server_name="/get_pick_place_pose_service"
|
||||
server_timeout="1000"/>
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="box4"
|
||||
GraspDirection = "y"
|
||||
PlaceDirection = "z"
|
||||
GraspPose="{GraspPose}"
|
||||
PostGraspPose="{PostGraspPose}"
|
||||
PostPostGraspPose="{PostPostGraspPose}"
|
||||
PreGraspPose="{PreGraspPose}"
|
||||
PlacePose="{PlacePose}"
|
||||
PrePlacePose="{PrePlacePose}"
|
||||
PostPlacePose="{PostPlacePose}"
|
||||
server_name="/get_pick_place_pose_service"
|
||||
server_timeout="1000"/>
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="box5"
|
||||
GraspDirection = "y"
|
||||
PlaceDirection = "z"
|
||||
GraspPose="{GraspPose}"
|
||||
PostGraspPose="{PostGraspPose}"
|
||||
PostPostGraspPose="{PostPostGraspPose}"
|
||||
PreGraspPose="{PreGraspPose}"
|
||||
PlacePose="{PlacePose}"
|
||||
PrePlacePose="{PrePlacePose}"
|
||||
PostPlacePose="{PostPlacePose}"
|
||||
server_name="/get_pick_place_pose_service"
|
||||
server_timeout="1000"/>
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="box6"
|
||||
GraspDirection = "y"
|
||||
PlaceDirection = "z"
|
||||
GraspPose="{GraspPose}"
|
||||
PostGraspPose="{PostGraspPose}"
|
||||
PostPostGraspPose="{PostPostGraspPose}"
|
||||
PreGraspPose="{PreGraspPose}"
|
||||
PlacePose="{PlacePose}"
|
||||
PrePlacePose="{PrePlacePose}"
|
||||
PostPlacePose="{PostPlacePose}"
|
||||
server_name="/get_pick_place_pose_service"
|
||||
server_timeout="1000"/>
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||
</Sequence>
|
||||
</Repeat>
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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){
|
||||
|
||||
|
|
|
|||
|
|
@ -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=[
|
||||
|
|
|
|||
|
|
@ -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(<dependency> 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
|
||||
)
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
geometry_msgs/Pose[] grasp
|
||||
geometry_msgs/Pose[] place
|
||||
|
|
@ -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")
|
||||
|
||||
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