Skip to content
Snippets Groups Projects
Commit c47afd31 authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

Merge branch 'feature/plan-only' into 'master'

added plan only functionality

See merge request ceti/ros-internal/panda_grasping!6
parents f1ac4675 9326ad1c
Branches
No related tags found
No related merge requests found
......@@ -76,7 +76,7 @@ public:
*/
bool pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id,
std::string object_to_pick_id);
std::string object_to_pick_id, bool plan_only = false);
/**
* Util method to place an object from above.
......@@ -88,7 +88,7 @@ public:
* @param object_to_place_id ID of the object to place
*/
bool placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,
double open_amount, std::string supporting_surface_id, std::string object_to_place_id);
double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only = false);
};
......
......@@ -74,7 +74,7 @@ bool pickObject(panda_grasping::PickObject::Request &req,
ROS_INFO("Picking up ...");
// 0.08 = 8cm = maximum gripper opening
if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object")) {
if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only)) {
res.success = false;
return false;
}
......@@ -121,7 +121,7 @@ placeObject(panda_grasping::PlaceObject::Request &req, panda_grasping::PlaceObje
}
ROS_INFO("Placing object.");
if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object")) {
if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only)) {
res.success = false;
return false;
}
......
......@@ -75,7 +75,7 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m
bool
GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
geometry_msgs::Vector3 dimensions,
double open_amount, std::string supporting_surface_id, std::string object_to_pick_id) {
double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) {
std::vector<moveit_msgs::Grasp> grasps;
grasps.resize(1);
......@@ -112,7 +112,8 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
}
move_group.setSupportSurfaceName(supporting_surface_id);
moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps);
moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only);
if (miec.val == miec.SUCCESS) {
return true;
......@@ -124,7 +125,7 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
bool
GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,
double open_amount, std::string supporting_surface_id, std::string object_to_place_id) {
double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only) {
std::vector<moveit_msgs::PlaceLocation> place_location;
place_location.resize(1);
......@@ -147,7 +148,8 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g
setupOpenGripper(place_location[0].post_place_posture, open_amount);
move_group.setSupportSurfaceName(supporting_surface_id);
moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location);
moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location, plan_only);
if (miec.val == miec.SUCCESS) {
return true;
......
geometry_msgs/Pose pose
geometry_msgs/Vector3 dimensions
bool plan_only
---
bool success
\ No newline at end of file
geometry_msgs/Pose pose
geometry_msgs/Vector3 dimensions
bool plan_only
---
bool success
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment