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