extension of services

include/grasp_util.h

@@ -92,12 +92,12 @@ public:
                         bool plan_only = false, int max_planning_retries = 1);
 
     /**
-     * TODO: TEST
-     * @param move_group
-     * @param object_pose_msg
-     * @param open_amount
-     * @param dimensions
-     * @return
+     * TODO: TEST / FIX
+     * @param move_group an initialized MoveGroupInterface instance
+     * @param object_pose_msg pose of the object to pick
+     * @param open_amount total opening amount (space between "forks")
+     * @param dimensions of the object
+     * @return true on success
      */
     bool pickObjectOnRobotfrontFromSide(moveit::planning_interface::MoveGroupInterface &move_group, std::string object_to_pick_id,
             geometry_msgs::Pose &object_pose_msg, double open_amount, geometry_msgs::Vector3 dimensions, int max_planning_retries = 1);

src/grasping/grasp_service.cpp

@@ -73,8 +73,10 @@ bool pickObject(panda_grasping::PickObject::Request &req,
         geometry_msgs::Pose pick_pose = grasp_util.getPickFromAbovePose(req.pose, req.dimensions, gripper_transform.rotation);
 
         ROS_INFO("Picking up ...");
+        int planing_retries = req.planing_retries <= 0 ? 1 : req.planing_retries;
+
         // 0.08 = 8cm = maximum gripper opening
-        if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only)) {
+        if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only, planing_retries)) {
             res.success = false;
             return false;
         }
@@ -121,7 +123,9 @@ 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", req.plan_only)) {
+        int planing_retries = req.planing_retries <= 0 ? 1 : req.planing_retries;
+
+        if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only, planing_retries)) {
             res.success = false;
             return false;
         }

src/grasping/grasp_util.cpp

@@ -75,7 +75,6 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m
 bool GraspUtil::pickObjectOnRobotfrontFromSide(moveit::planning_interface::MoveGroupInterface &move_group, std::string object_to_pick_id,
         geometry_msgs::Pose &object_pose_msg, double open_amount, geometry_msgs::Vector3 dimensions, int max_planning_retries) {
 
-
     std::vector<moveit_msgs::Grasp> grasps;
     grasps.resize(1);
 

srv/PickObject.srv

 geometry_msgs/Pose pose
 geometry_msgs/Vector3 dimensions
 bool plan_only
+int8 planing_retries
 ---
 bool success
\ No newline at end of file

srv/PlaceObject.srv

 geometry_msgs/Pose pose
 geometry_msgs/Vector3 dimensions
 bool plan_only
+int8 planing_retries
 ---
 bool success
\ No newline at end of file