From 9326ad1c8346b4adfe2d13d1964f9e17b686a536 Mon Sep 17 00:00:00 2001 From: SebastianEbert <sebastian.ebert@tu-dresden.de> Date: Mon, 3 May 2021 10:40:39 +0200 Subject: [PATCH] fixes from merge request --- src/grasping/grasp_service.cpp | 4 ++-- src/grasping/grasp_util.cpp | 16 ++-------------- srv/PickObject.srv | 1 + srv/PlaceObject.srv | 1 + 4 files changed, 6 insertions(+), 16 deletions(-) diff --git a/src/grasping/grasp_service.cpp b/src/grasping/grasp_service.cpp index 5da6b4a..7e27a2a 100644 --- a/src/grasping/grasp_service.cpp +++ b/src/grasping/grasp_service.cpp @@ -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; } diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp index 0a86985..e8d90c2 100644 --- a/src/grasping/grasp_util.cpp +++ b/src/grasping/grasp_util.cpp @@ -113,13 +113,7 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr move_group.setSupportSurfaceName(supporting_surface_id); - moveit_msgs::MoveItErrorCodes miec; - - if(plan_only){ - miec = move_group.pick(object_to_pick_id, grasps, true); - }else{ - 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; @@ -155,13 +149,7 @@ 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; - - if(plan_only){ - miec = move_group.place(object_to_place_id, place_location, true); - }else{ - 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; diff --git a/srv/PickObject.srv b/srv/PickObject.srv index d0e99d0..a7f8eb4 100644 --- a/srv/PickObject.srv +++ b/srv/PickObject.srv @@ -1,4 +1,5 @@ geometry_msgs/Pose pose geometry_msgs/Vector3 dimensions +bool plan_only --- bool success \ No newline at end of file diff --git a/srv/PlaceObject.srv b/srv/PlaceObject.srv index d0e99d0..a7f8eb4 100644 --- a/srv/PlaceObject.srv +++ b/srv/PlaceObject.srv @@ -1,4 +1,5 @@ geometry_msgs/Pose pose geometry_msgs/Vector3 dimensions +bool plan_only --- bool success \ No newline at end of file -- GitLab